From 0038d346a940f903bb0c91fc411b9174c5d39398 Mon Sep 17 00:00:00 2001 From: pomerlef Date: Wed, 20 Jul 2016 13:23:15 -0400 Subject: [PATCH 01/31] add new header file for point-to-point error --- include/steam.hpp | 1 + .../samples/PointToPointErrorEval.hpp | 71 +++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 include/steam/evaluator/samples/PointToPointErrorEval.hpp diff --git a/include/steam.hpp b/include/steam.hpp index c326630..a0dda44 100644 --- a/include/steam.hpp +++ b/include/steam.hpp @@ -34,6 +34,7 @@ #include #include #include +#include // evaluator - block auto diff #include diff --git a/include/steam/evaluator/samples/PointToPointErrorEval.hpp b/include/steam/evaluator/samples/PointToPointErrorEval.hpp new file mode 100644 index 0000000..f3069db --- /dev/null +++ b/include/steam/evaluator/samples/PointToPointErrorEval.hpp @@ -0,0 +1,71 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file PointToPointErrorEval.hpp +/// +/// \author Francois Pomerleau, ASRL +/// \brief This evaluator was develop in the context of ICP (Iterative Closest Point) +/// implementation. +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_POINT_TO_POINT_ERROR_EVALUATOR_HPP +#define STEAM_POINT_TO_POINT_ERROR_EVALUATOR_HPP + +#include + +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief The distance between two points living in their respective frame is used as our +/// error function. +/// +////////////////////////////////////////////////////////////////////////////////////////////// +class PointToPointErrorEval : public ErrorEvaluator<3,6>::type +{ +public: + + /// Convenience typedefs + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + /// \param point1 A point express in euclidean coordinates (i.e., [x, y, z]) expressed + /// in frame 1. + /// \param T_world_1 Transformation matrix from frame 1 to frame world. + /// \param point2 A point express in euclidean coordinates (i.e., [x, y, z]) expressed + /// in frame 2. + /// \param T_world_2 Transformation matrix from frame 2 to frame world. + ////////////////////////////////////////////////////////////////////////////////////////////// + PointToPointErrorEval(const Eigen::Vector3d& point1, + const se3::TransformEvaluator::ConstPtr& T_world_1, + const Eigen::Vector3d& point2, + const se3::TransformEvaluator::ConstPtr& T_world_2 + ); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; //TODO: check if we need to define that + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the 3-d measurement error (x, y, z) + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Vector3d evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the 3-d measurement error (x, y, z) and Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Vector3d evaluate(const Eigen::Matrix3d& lhs, + std::vector >* jacs) const; + +private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Point evaluator (evaluates the point transformed into the camera frame) + ////////////////////////////////////////////////////////////////////////////////////////////// + se3::ComposeLandmarkEvaluator::ConstPtr eval_; + +}; + +} // steam + +#endif // STEAM_STEREO_CAMERA_ERROR_EVALUATOR_HPP From bc1f9442eff33ad8908ce4f7de9f599ae47bcd65 Mon Sep 17 00:00:00 2001 From: pomerlef Date: Thu, 21 Jul 2016 16:05:45 -0400 Subject: [PATCH 02/31] finish PointToPointErrorEval first draft and setup placeholder for unit test --- .gitignore | 1 + .../samples/PointToPointErrorEval.hpp | 38 +++++----- .../samples/PointToPointErrorEval.cpp | 75 +++++++++++++++++++ tests/CMakeLists.txt | 1 + tests/evaluator_test.cpp | 19 +++++ 5 files changed, 116 insertions(+), 18 deletions(-) create mode 100644 src/evaluator/samples/PointToPointErrorEval.cpp create mode 100644 tests/evaluator_test.cpp diff --git a/.gitignore b/.gitignore index b4f1a10..3b9b0c7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ build *~ .idea +.swp diff --git a/include/steam/evaluator/samples/PointToPointErrorEval.hpp b/include/steam/evaluator/samples/PointToPointErrorEval.hpp index f3069db..e3a0ffa 100644 --- a/include/steam/evaluator/samples/PointToPointErrorEval.hpp +++ b/include/steam/evaluator/samples/PointToPointErrorEval.hpp @@ -18,7 +18,7 @@ namespace steam { /// error function. /// ////////////////////////////////////////////////////////////////////////////////////////////// -class PointToPointErrorEval : public ErrorEvaluator<3,6>::type +class PointToPointErrorEval : public ErrorEvaluator<4,6>::type { public: @@ -28,17 +28,17 @@ class PointToPointErrorEval : public ErrorEvaluator<3,6>::type ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor - /// \param point1 A point express in euclidean coordinates (i.e., [x, y, z]) expressed - /// in frame 1. - /// \param T_world_1 Transformation matrix from frame 1 to frame world. - /// \param point2 A point express in euclidean coordinates (i.e., [x, y, z]) expressed - /// in frame 2. - /// \param T_world_2 Transformation matrix from frame 2 to frame world. + /// \param ref_a A point from the reference point cloud (static) expressed in homogeneous + /// coordinates (i.e., [x, y, z, 1]) and in the frame a. + /// \param T_a_world Transformation matrix from frame world to frame a. + /// \param read_b A point from the reading point cloud expressed in homogeneous + /// coordinates (i.e., [x, y, z, 1]) and in the frame b. + /// \param T_b_world Transformation matrix from frame world to frame b. ////////////////////////////////////////////////////////////////////////////////////////////// - PointToPointErrorEval(const Eigen::Vector3d& point1, - const se3::TransformEvaluator::ConstPtr& T_world_1, - const Eigen::Vector3d& point2, - const se3::TransformEvaluator::ConstPtr& T_world_2 + PointToPointErrorEval(const Eigen::Vector4d& ref_a, + const se3::TransformEvaluator::ConstPtr& T_a_world, + const Eigen::Vector4d& read_b, + const se3::TransformEvaluator::ConstPtr& T_b_world ); ////////////////////////////////////////////////////////////////////////////////////////////// @@ -47,25 +47,27 @@ class PointToPointErrorEval : public ErrorEvaluator<3,6>::type virtual bool isActive() const; //TODO: check if we need to define that ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Evaluate the 3-d measurement error (x, y, z) + /// \brief Evaluate the 4-d measurement error (x, y, z, 0) ////////////////////////////////////////////////////////////////////////////////////////////// - virtual Eigen::Vector3d evaluate() const; + virtual Eigen::Vector4d evaluate() const; ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Evaluate the 3-d measurement error (x, y, z) and Jacobians + /// \brief Evaluate the 4-d measurement error (x, y, z, 0) and Jacobians ////////////////////////////////////////////////////////////////////////////////////////////// - virtual Eigen::Vector3d evaluate(const Eigen::Matrix3d& lhs, - std::vector >* jacs) const; + virtual Eigen::Vector4d evaluate(const Eigen::Matrix4d& lhs, + std::vector >* jacs) const; private: ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Point evaluator (evaluates the point transformed into the camera frame) ////////////////////////////////////////////////////////////////////////////////////////////// - se3::ComposeLandmarkEvaluator::ConstPtr eval_; + Eigen::Vector4d ref_a_; + se3::ComposeInverseTransformEvaluator::ConstPtr T_ab_; + Eigen::Vector4d read_b_; }; } // steam -#endif // STEAM_STEREO_CAMERA_ERROR_EVALUATOR_HPP +#endif // STEAM_POINT_TO_POINT_ERROR_EVALUATOR_HPP diff --git a/src/evaluator/samples/PointToPointErrorEval.cpp b/src/evaluator/samples/PointToPointErrorEval.cpp new file mode 100644 index 0000000..fc49de2 --- /dev/null +++ b/src/evaluator/samples/PointToPointErrorEval.cpp @@ -0,0 +1,75 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file PointToPointErrorEval.cpp +/// +/// \author Francois Pomerleau, ASRL +/// \brief This evaluator was develop in the context of ICP (Iterative Closest Point) +/// implementation. +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +namespace steam { + + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +PointToPointErrorEval::PointToPointErrorEval(const Eigen::Vector4d& ref_a, + const se3::TransformEvaluator::ConstPtr& T_a_world, + const Eigen::Vector4d& read_b, + const se3::TransformEvaluator::ConstPtr& T_b_world + ) + : ref_a_(ref_a), + T_ab_(se3::ComposeInverseTransformEvaluator::MakeShared(T_a_world, T_b_world)), + read_b_(read_b) { +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool PointToPointErrorEval::isActive() const { + return T_ab_->isActive(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the 4-d measurement error (ul vl ur vr) +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Vector4d PointToPointErrorEval::evaluate() const { + + // Return error (between measurement and point estimate projected in camera frame) + return ref_a_ - (T_ab_->evaluate() * read_b_); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the 4-d measurement error (ul vl ur vr) and Jacobians +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Vector4d PointToPointErrorEval::evaluate(const Eigen::Matrix4d& lhs, std::vector >* jacs) const { + + // Check and initialize jacobian array + if (jacs == NULL) { + throw std::invalid_argument("Null pointer provided to return-input 'jacs' in evaluate"); + } + jacs->clear(); + + // Get evaluation tree + EvalTreeHandle blkAutoTransform = + T_ab_->getBlockAutomaticEvaluation(); + + // Get evaluation from tree + //TODO: why just not using T_ab_->evaluate() instead? + const lgmath::se3::Transformation T_ab = blkAutoTransform.getValue(); + const Eigen::Vector4d read_a = T_ab * read_b_; + + // Get Jacobians + //TODO: why point2fs(...) doesn't take Vector4d as input? + //TODO: use (-) instead of (-1) + //TODO: why is the newLhs not the same shape as lhs (input)? + Eigen::Matrix newLhs = (-1)*lhs*lgmath::se3::point2fs(read_a.head<3>()); + T_ab_->appendBlockAutomaticJacobians(newLhs, blkAutoTransform.getRoot(), jacs); + + // Return evaluation + return ref_a_ - read_a; +} + + +} // steam diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 29b206d..b6aabc0 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -36,6 +36,7 @@ add_executable(steam_unit_tests ${CMAKE_CURRENT_SOURCE_DIR}/sample_test.cpp ${CMAKE_CURRENT_SOURCE_DIR}/time_test.cpp ${CMAKE_CURRENT_SOURCE_DIR}/pattern_test.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/evaluator_test.cpp ) target_link_libraries(steam_unit_tests steam ${DEPEND_LIBS}) diff --git a/tests/evaluator_test.cpp b/tests/evaluator_test.cpp new file mode 100644 index 0000000..d53693b --- /dev/null +++ b/tests/evaluator_test.cpp @@ -0,0 +1,19 @@ +#include "catch.hpp" + +///////////////////////////////////////////////////////////////////////////////////////////// +/// ErrorEvaluator Test +///////////////////////////////////////////////////////////////////////////////////////////// +TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { + + int a = 5; + REQUIRE( a == 5 ); + + SECTION("if we change a to 10" ) { + a = 10; + REQUIRE( a == 10 ); + } + + SECTION("in a later section it is still 5" ) { + REQUIRE( a == 5 ); + } +} // TEST_CASE From a16f7926a6e29c8694363a967cbaa64d6e9b7e1d Mon Sep 17 00:00:00 2001 From: pomerlef Date: Thu, 21 Jul 2016 16:08:05 -0400 Subject: [PATCH 03/31] change gitignore to add vim backfiles --- .gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 3b9b0c7..929834e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ build *~ .idea -.swp +*.swp From 9475fd6e9da8b0e37392ee7fdd80717fa04135f6 Mon Sep 17 00:00:00 2001 From: pomerlef Date: Thu, 21 Jul 2016 18:01:59 -0400 Subject: [PATCH 04/31] finished a simple unit test --- tests/evaluator_test.cpp | 116 ++++++++++++++++++++++++++++++++++++--- 1 file changed, 107 insertions(+), 9 deletions(-) diff --git a/tests/evaluator_test.cpp b/tests/evaluator_test.cpp index d53693b..87f681a 100644 --- a/tests/evaluator_test.cpp +++ b/tests/evaluator_test.cpp @@ -1,19 +1,117 @@ #include "catch.hpp" +#include "steam/steam.hpp" ///////////////////////////////////////////////////////////////////////////////////////////// /// ErrorEvaluator Test +/// \author Francois Pomerleau ///////////////////////////////////////////////////////////////////////////////////////////// + +Eigen::Vector4d initVector4d(double a,double b, double c, double d) +{ + Eigen::Vector4d v; + v << a, b, c, d; + + return v; +} + + + + TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { - int a = 5; - REQUIRE( a == 5 ); + //--------------------------------------- + // 1- Steam state variables + //--------------------------------------- + + // Setup state for the reference frame + //TODO: add identity here + steam::se3::TransformStateVar::Ptr stateReference(new steam::se3::TransformStateVar()); + // Lock the reference frame + stateReference->setLock(true); + + // Setup state for the reading frame + steam::se3::TransformStateVar::Ptr stateReading(new steam::se3::TransformStateVar()); + + //--------------------------------------- + // 2- Steam cost terms + //--------------------------------------- + + // steam cost terms + steam::ParallelizedCostTermCollection::Ptr costTerms(new steam::ParallelizedCostTermCollection()); + + + // Set the NoiseModel (R_i) to identity + steam::BaseNoiseModel<4>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<4>(Eigen::MatrixXd::Identity(4,4))); + + // Set the LossFunction to L2 (least-squared) + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + + // Build simple point cloud + const Eigen::Vector4d ref_0 = initVector4d(0, 0, 0, 1); + const Eigen::Vector4d ref_1 = initVector4d(1, 0, 0, 1); + const Eigen::Vector4d ref_2 = initVector4d(0, 1, 0, 1); + + Eigen::Matrix4d T_gt; + T_gt << 1, 0, 0, 1, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1; + + Eigen::Vector4d read_0 = T_gt * ref_0; + Eigen::Vector4d read_1 = T_gt * ref_1; + Eigen::Vector4d read_2 = T_gt * ref_2; + + auto sharedStateReference = steam::se3::TransformStateEvaluator::MakeShared(stateReference); + auto sharedStateReading = steam::se3::TransformStateEvaluator::MakeShared(stateReading); + + + steam::PointToPointErrorEval::Ptr errorfunc_0(new steam::PointToPointErrorEval(ref_0, sharedStateReference, read_0, sharedStateReading)); + steam::PointToPointErrorEval::Ptr errorfunc_1(new steam::PointToPointErrorEval(ref_1, sharedStateReference, read_1, sharedStateReading)); + steam::PointToPointErrorEval::Ptr errorfunc_2(new steam::PointToPointErrorEval(ref_2, sharedStateReference, read_2, sharedStateReading)); + + steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_0(new steam::WeightedLeastSqCostTerm<4,6>(errorfunc_0, sharedNoiseModel, sharedLossFunc)); + steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_1(new steam::WeightedLeastSqCostTerm<4,6>(errorfunc_1, sharedNoiseModel, sharedLossFunc)); + steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_2(new steam::WeightedLeastSqCostTerm<4,6>(errorfunc_2, sharedNoiseModel, sharedLossFunc)); + + + costTerms->add(cost_0); + costTerms->add(cost_1); + costTerms->add(cost_2); + + + //--------------------------------------- + // 3- Optimization problem + //--------------------------------------- + + steam::OptimizationProblem problem; + + // Add states + problem.addStateVariable(stateReference); + problem.addStateVariable(stateReading); + + // Add cost terms + problem.addCostTerm(costTerms); + + //--------------------------------------- + // 4- Solve + //--------------------------------------- + + //typedef steam::DoglegGaussNewtonSolver SolverType + typedef steam::VanillaGaussNewtonSolver SolverType; + + SolverType::Params params; + params.verbose = true; + //params.maxIterations = 500; + //params.absoluteCostThreshold = 0.0; + //params.absoluteCostChangeThreshold = 1e-4; + //params.relativeCostChangeThreshold = 1e-4; + + // Make solver + SolverType solver(&problem, params); + + // Optimize + solver.optimize(); - SECTION("if we change a to 10" ) { - a = 10; - REQUIRE( a == 10 ); - } + std::cout << stateReading->getValue() << std::endl; - SECTION("in a later section it is still 5" ) { - REQUIRE( a == 5 ); - } } // TEST_CASE From b3471ccc1b1020dff20a245d27df2a0a2bf3d0b3 Mon Sep 17 00:00:00 2001 From: pomerlef Date: Fri, 22 Jul 2016 17:32:54 -0400 Subject: [PATCH 05/31] full problem solving unit test for PointToPointErrorEval --- .../samples/PointToPointErrorEval.cpp | 54 +++--- tests/evaluator_test.cpp | 156 +++++++++++++----- 2 files changed, 146 insertions(+), 64 deletions(-) diff --git a/src/evaluator/samples/PointToPointErrorEval.cpp b/src/evaluator/samples/PointToPointErrorEval.cpp index fc49de2..9e2c9a8 100644 --- a/src/evaluator/samples/PointToPointErrorEval.cpp +++ b/src/evaluator/samples/PointToPointErrorEval.cpp @@ -1,3 +1,4 @@ +// vim: ts=4:sw=4:noexpandtab ////////////////////////////////////////////////////////////////////////////////////////////// /// \file PointToPointErrorEval.cpp /// @@ -14,21 +15,21 @@ namespace steam { ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor ////////////////////////////////////////////////////////////////////////////////////////////// -PointToPointErrorEval::PointToPointErrorEval(const Eigen::Vector4d& ref_a, - const se3::TransformEvaluator::ConstPtr& T_a_world, - const Eigen::Vector4d& read_b, - const se3::TransformEvaluator::ConstPtr& T_b_world - ) - : ref_a_(ref_a), - T_ab_(se3::ComposeInverseTransformEvaluator::MakeShared(T_a_world, T_b_world)), - read_b_(read_b) { +PointToPointErrorEval::PointToPointErrorEval( + const Eigen::Vector4d& ref_a, + const se3::TransformEvaluator::ConstPtr& T_a_world, + const Eigen::Vector4d& read_b, + const se3::TransformEvaluator::ConstPtr& T_b_world + ): ref_a_(ref_a), + T_ab_(se3::ComposeInverseTransformEvaluator::MakeShared(T_a_world, T_b_world)), + read_b_(read_b) { } ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Returns whether or not an evaluator contains unlocked state variables ////////////////////////////////////////////////////////////////////////////////////////////// bool PointToPointErrorEval::isActive() const { - return T_ab_->isActive(); + return T_ab_->isActive(); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -36,8 +37,8 @@ bool PointToPointErrorEval::isActive() const { ////////////////////////////////////////////////////////////////////////////////////////////// Eigen::Vector4d PointToPointErrorEval::evaluate() const { - // Return error (between measurement and point estimate projected in camera frame) - return ref_a_ - (T_ab_->evaluate() * read_b_); + // Return error (between measurement and point estimate projected in camera frame) + return ref_a_ - (T_ab_->evaluate() * read_b_); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -45,30 +46,29 @@ Eigen::Vector4d PointToPointErrorEval::evaluate() const { ////////////////////////////////////////////////////////////////////////////////////////////// Eigen::Vector4d PointToPointErrorEval::evaluate(const Eigen::Matrix4d& lhs, std::vector >* jacs) const { - // Check and initialize jacobian array - if (jacs == NULL) { - throw std::invalid_argument("Null pointer provided to return-input 'jacs' in evaluate"); - } - jacs->clear(); + // Check and initialize jacobian array + if (jacs == NULL) { + throw std::invalid_argument("Null pointer provided to return-input 'jacs' in evaluate"); + } + jacs->clear(); - // Get evaluation tree - EvalTreeHandle blkAutoTransform = - T_ab_->getBlockAutomaticEvaluation(); + // Get evaluation tree + EvalTreeHandle blkAutoTransform = + T_ab_->getBlockAutomaticEvaluation(); - // Get evaluation from tree + // Get evaluation from tree //TODO: why just not using T_ab_->evaluate() instead? - const lgmath::se3::Transformation T_ab = blkAutoTransform.getValue(); + const lgmath::se3::Transformation T_ab = blkAutoTransform.getValue(); const Eigen::Vector4d read_a = T_ab * read_b_; - // Get Jacobians + // Get Jacobians //TODO: why point2fs(...) doesn't take Vector4d as input? - //TODO: use (-) instead of (-1) //TODO: why is the newLhs not the same shape as lhs (input)? - Eigen::Matrix newLhs = (-1)*lhs*lgmath::se3::point2fs(read_a.head<3>()); - T_ab_->appendBlockAutomaticJacobians(newLhs, blkAutoTransform.getRoot(), jacs); + Eigen::Matrix newLhs = -lhs*lgmath::se3::point2fs(read_a.head<3>()); + T_ab_->appendBlockAutomaticJacobians(newLhs, blkAutoTransform.getRoot(), jacs); - // Return evaluation - return ref_a_ - read_a; + // Return evaluation + return ref_a_ - read_a; } diff --git a/tests/evaluator_test.cpp b/tests/evaluator_test.cpp index 87f681a..6821976 100644 --- a/tests/evaluator_test.cpp +++ b/tests/evaluator_test.cpp @@ -1,30 +1,65 @@ -#include "catch.hpp" -#include "steam/steam.hpp" +// vim: ts=4:sw=4:noexpandtab ///////////////////////////////////////////////////////////////////////////////////////////// /// ErrorEvaluator Test /// \author Francois Pomerleau +/// \brief To execute only this test run: +/// $ ./tests/steam_unit_tests PointToPointErrorEval -s ///////////////////////////////////////////////////////////////////////////////////////////// -Eigen::Vector4d initVector4d(double a,double b, double c, double d) + +#include "catch.hpp" +#include "steam/steam.hpp" +#include + +// Helper function to initialize a 3D point in homogeneous coordinates +Eigen::Vector4d initVector4d(double x, double y, double z) { Eigen::Vector4d v; - v << a, b, c, d; + v << x, y, z, 1.0; return v; } +// Helper function to solve and check that the solution is close +// to the ground truth transformation used to generate the data +void solveSimpleProblem(Eigen::Matrix T_components) +{ + //--------------------------------------- + // General structure: + // 0- Generate simple point clouds + // 1- Steam state variables + // 2- Steam cost terms + // 3- Set up the optimization problem + // 4- Solve + // 5- Check that the transformation are the same + //--------------------------------------- + //--------------------------------------- + // 0- Generate simple point clouds + //--------------------------------------- -TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { + // Build a fixed point cloud (reference) with 3 points + const Eigen::Vector4d ref_0 = initVector4d(0, 0, 0); + const Eigen::Vector4d ref_1 = initVector4d(1, 0, 0); + const Eigen::Vector4d ref_2 = initVector4d(0, 1, 0); + // Build a transformation matrix with a translation on x-axis + lgmath::se3::Transformation T_gt(T_components); + + // Move the reference point cloud to generate a + // second point cloud called reading + Eigen::Vector4d read_0 = T_gt.matrix() * ref_0; + Eigen::Vector4d read_1 = T_gt.matrix() * ref_1; + Eigen::Vector4d read_2 = T_gt.matrix() * ref_2; + //--------------------------------------- // 1- Steam state variables //--------------------------------------- // Setup state for the reference frame - //TODO: add identity here + // Note: the default constructor sets the state to identity steam::se3::TransformStateVar::Ptr stateReference(new steam::se3::TransformStateVar()); // Lock the reference frame stateReference->setLock(true); @@ -39,48 +74,34 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { // steam cost terms steam::ParallelizedCostTermCollection::Ptr costTerms(new steam::ParallelizedCostTermCollection()); - // Set the NoiseModel (R_i) to identity - steam::BaseNoiseModel<4>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<4>(Eigen::MatrixXd::Identity(4,4))); + steam::BaseNoiseModel<4>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<4>(Eigen::Matrix4d::Identity())); // Set the LossFunction to L2 (least-squared) steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); - // Build simple point cloud - const Eigen::Vector4d ref_0 = initVector4d(0, 0, 0, 1); - const Eigen::Vector4d ref_1 = initVector4d(1, 0, 0, 1); - const Eigen::Vector4d ref_2 = initVector4d(0, 1, 0, 1); - - Eigen::Matrix4d T_gt; - T_gt << 1, 0, 0, 1, - 0, 1, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1; - - Eigen::Vector4d read_0 = T_gt * ref_0; - Eigen::Vector4d read_1 = T_gt * ref_1; - Eigen::Vector4d read_2 = T_gt * ref_2; - - auto sharedStateReference = steam::se3::TransformStateEvaluator::MakeShared(stateReference); - auto sharedStateReading = steam::se3::TransformStateEvaluator::MakeShared(stateReading); - - - steam::PointToPointErrorEval::Ptr errorfunc_0(new steam::PointToPointErrorEval(ref_0, sharedStateReference, read_0, sharedStateReading)); - steam::PointToPointErrorEval::Ptr errorfunc_1(new steam::PointToPointErrorEval(ref_1, sharedStateReference, read_1, sharedStateReading)); - steam::PointToPointErrorEval::Ptr errorfunc_2(new steam::PointToPointErrorEval(ref_2, sharedStateReference, read_2, sharedStateReading)); - - steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_0(new steam::WeightedLeastSqCostTerm<4,6>(errorfunc_0, sharedNoiseModel, sharedLossFunc)); - steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_1(new steam::WeightedLeastSqCostTerm<4,6>(errorfunc_1, sharedNoiseModel, sharedLossFunc)); - steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_2(new steam::WeightedLeastSqCostTerm<4,6>(errorfunc_2, sharedNoiseModel, sharedLossFunc)); + // Convert our states to Evaluators + auto stateReferenceEvaluator = steam::se3::TransformStateEvaluator::MakeShared(stateReference); + auto stateReadingEvaluator = steam::se3::TransformStateEvaluator::MakeShared(stateReading); + // Build the alignment errors + steam::PointToPointErrorEval::Ptr error_0(new steam::PointToPointErrorEval(ref_0, stateReferenceEvaluator, read_0, stateReadingEvaluator)); + steam::PointToPointErrorEval::Ptr error_1(new steam::PointToPointErrorEval(ref_1, stateReferenceEvaluator, read_1, stateReadingEvaluator)); + steam::PointToPointErrorEval::Ptr error_2(new steam::PointToPointErrorEval(ref_2, stateReferenceEvaluator, read_2, stateReadingEvaluator)); + + // Build the cost terms + steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_0(new steam::WeightedLeastSqCostTerm<4,6>(error_0, sharedNoiseModel, sharedLossFunc)); + steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_1(new steam::WeightedLeastSqCostTerm<4,6>(error_1, sharedNoiseModel, sharedLossFunc)); + steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_2(new steam::WeightedLeastSqCostTerm<4,6>(error_2, sharedNoiseModel, sharedLossFunc)); + // Add our individual cost terms to the collection costTerms->add(cost_0); costTerms->add(cost_1); costTerms->add(cost_2); //--------------------------------------- - // 3- Optimization problem + // 3- Set up the optimization problem //--------------------------------------- steam::OptimizationProblem problem; @@ -92,6 +113,7 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { // Add cost terms problem.addCostTerm(costTerms); + //--------------------------------------- // 4- Solve //--------------------------------------- @@ -100,7 +122,7 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { typedef steam::VanillaGaussNewtonSolver SolverType; SolverType::Params params; - params.verbose = true; + params.verbose = false; //params.maxIterations = 500; //params.absoluteCostThreshold = 0.0; //params.absoluteCostChangeThreshold = 1e-4; @@ -112,6 +134,66 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { // Optimize solver.optimize(); - std::cout << stateReading->getValue() << std::endl; + //--------------------------------------- + // 5- Check that the transformation are the same + //--------------------------------------- + INFO("Minimized transformation:" << "\n" << + stateReading->getValue().matrix() << "\n" << + "is different than original transformation:" << "\n" << + T_gt.matrix() << "\n" << + "difference being:" << "\n" << + stateReading->getValue().matrix() - T_gt.matrix() + ); + + // Confirm that our state is the same as our ground truth transformation + CHECK(lgmath::common::nearEqual(stateReading->getValue().matrix(), + T_gt.matrix(), + 1e-3 + )); + +} + + +TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { + + Eigen::Matrix T_components; + + SECTION("Simple translation") + { + T_components << 1.0, // translation x + 0.0, // translation y + 0.0, // translation z + 0.0, // rotation around x-axis + 0.0, // rotation around y-axis + 0.0; // rotation around z-axis + + solveSimpleProblem(T_components); + } + + SECTION("Simple rotation") + { + T_components << 0.0, // translation x + 0.0, // translation y + 0.0, // translation z + 1.0, // rotation around x-axis + 0.0, // rotation around y-axis + 0.0; // rotation around z-axis + + solveSimpleProblem(T_components); + } + + SECTION("Random transformation (1000)") + { + srand((unsigned int) time(0)); + + for(int i=0; i<1000; i++) + { + // random numbers in interval [-1, 1] + T_components = Eigen::Matrix::Random(); + solveSimpleProblem(T_components); + } + + } + } // TEST_CASE From 08f810104d4665e6a8971b2451f77da18f2f0581 Mon Sep 17 00:00:00 2001 From: pomerlef Date: Mon, 25 Jul 2016 17:08:42 -0400 Subject: [PATCH 06/31] change unit test --- tests/evaluator_test.cpp | 61 ++++++++++++++++++++++++---------------- 1 file changed, 37 insertions(+), 24 deletions(-) diff --git a/tests/evaluator_test.cpp b/tests/evaluator_test.cpp index 6821976..1b4dfa9 100644 --- a/tests/evaluator_test.cpp +++ b/tests/evaluator_test.cpp @@ -41,18 +41,23 @@ void solveSimpleProblem(Eigen::Matrix T_components) //--------------------------------------- // Build a fixed point cloud (reference) with 3 points - const Eigen::Vector4d ref_0 = initVector4d(0, 0, 0); - const Eigen::Vector4d ref_1 = initVector4d(1, 0, 0); - const Eigen::Vector4d ref_2 = initVector4d(0, 1, 0); - - // Build a transformation matrix with a translation on x-axis - lgmath::se3::Transformation T_gt(T_components); + // expressed in frame a + const Eigen::Vector4d ref_a_0 = initVector4d(0, 0, 0); + const Eigen::Vector4d ref_a_1 = initVector4d(1, 0, 0); + const Eigen::Vector4d ref_a_2 = initVector4d(0, 1, 0); + + // Build a ground truth (gt) transformation matrix + // from frame b to frame a + const lgmath::se3::Transformation Tgt_a_b(T_components); + // and its inverse + const lgmath::se3::Transformation Tgt_b_a = Tgt_a_b.inverse(); + // Move the reference point cloud to generate a - // second point cloud called reading - Eigen::Vector4d read_0 = T_gt.matrix() * ref_0; - Eigen::Vector4d read_1 = T_gt.matrix() * ref_1; - Eigen::Vector4d read_2 = T_gt.matrix() * ref_2; + // second point cloud called read (reading) + Eigen::Vector4d read_b_0 = Tgt_b_a.matrix() * ref_a_0; + Eigen::Vector4d read_b_1 = Tgt_b_a.matrix() * ref_a_1; + Eigen::Vector4d read_b_2 = Tgt_b_a.matrix() * ref_a_2; //--------------------------------------- // 1- Steam state variables @@ -74,25 +79,33 @@ void solveSimpleProblem(Eigen::Matrix T_components) // steam cost terms steam::ParallelizedCostTermCollection::Ptr costTerms(new steam::ParallelizedCostTermCollection()); + // Convert our states to Transform Evaluators + // T_a_a is silly (most be identity) but it's there for completness + auto T_a_a = steam::se3::TransformStateEvaluator::MakeShared(stateReference); + auto T_a_b = steam::se3::TransformStateEvaluator::MakeShared(stateReading); + + // Define our error funtion + typedef steam::PointToPointErrorEval Error; + + // Build the alignment errors + Error::Ptr error_0(new Error(ref_a_0, T_a_a, read_b_0, T_a_b)); + Error::Ptr error_1(new Error(ref_a_1, T_a_a, read_b_1, T_a_b)); + Error::Ptr error_2(new Error(ref_a_2, T_a_a, read_b_2, T_a_b)); + + // Set the NoiseModel (R_i) to identity steam::BaseNoiseModel<4>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<4>(Eigen::Matrix4d::Identity())); // Set the LossFunction to L2 (least-squared) steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); - // Convert our states to Evaluators - auto stateReferenceEvaluator = steam::se3::TransformStateEvaluator::MakeShared(stateReference); - auto stateReadingEvaluator = steam::se3::TransformStateEvaluator::MakeShared(stateReading); - - // Build the alignment errors - steam::PointToPointErrorEval::Ptr error_0(new steam::PointToPointErrorEval(ref_0, stateReferenceEvaluator, read_0, stateReadingEvaluator)); - steam::PointToPointErrorEval::Ptr error_1(new steam::PointToPointErrorEval(ref_1, stateReferenceEvaluator, read_1, stateReadingEvaluator)); - steam::PointToPointErrorEval::Ptr error_2(new steam::PointToPointErrorEval(ref_2, stateReferenceEvaluator, read_2, stateReadingEvaluator)); + // Define our cost term + typedef steam::WeightedLeastSqCostTerm<4,6> Cost; // Build the cost terms - steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_0(new steam::WeightedLeastSqCostTerm<4,6>(error_0, sharedNoiseModel, sharedLossFunc)); - steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_1(new steam::WeightedLeastSqCostTerm<4,6>(error_1, sharedNoiseModel, sharedLossFunc)); - steam::WeightedLeastSqCostTerm<4,6>::Ptr cost_2(new steam::WeightedLeastSqCostTerm<4,6>(error_2, sharedNoiseModel, sharedLossFunc)); + Cost::Ptr cost_0(new Cost(error_0, sharedNoiseModel, sharedLossFunc)); + Cost::Ptr cost_1(new Cost(error_1, sharedNoiseModel, sharedLossFunc)); + Cost::Ptr cost_2(new Cost(error_2, sharedNoiseModel, sharedLossFunc)); // Add our individual cost terms to the collection costTerms->add(cost_0); @@ -140,14 +153,14 @@ void solveSimpleProblem(Eigen::Matrix T_components) INFO("Minimized transformation:" << "\n" << stateReading->getValue().matrix() << "\n" << "is different than original transformation:" << "\n" << - T_gt.matrix() << "\n" << + Tgt_b_a.matrix() << "\n" << "difference being:" << "\n" << - stateReading->getValue().matrix() - T_gt.matrix() + stateReading->getValue().matrix() - Tgt_b_a.matrix() ); // Confirm that our state is the same as our ground truth transformation CHECK(lgmath::common::nearEqual(stateReading->getValue().matrix(), - T_gt.matrix(), + Tgt_b_a.matrix(), 1e-3 )); From 9e6c3286a339f4c39ae91089ab6ab0a4ab8e3073 Mon Sep 17 00:00:00 2001 From: TangTim Date: Wed, 3 Aug 2016 14:37:11 -0400 Subject: [PATCH 07/31] Add files via upload --- tests/evaluator_test.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/tests/evaluator_test.cpp b/tests/evaluator_test.cpp index 1b4dfa9..03a8bef 100644 --- a/tests/evaluator_test.cpp +++ b/tests/evaluator_test.cpp @@ -9,7 +9,7 @@ #include "catch.hpp" -#include "steam/steam.hpp" +#include "steam.hpp" #include // Helper function to initialize a 3D point in homogeneous coordinates @@ -83,14 +83,15 @@ void solveSimpleProblem(Eigen::Matrix T_components) // T_a_a is silly (most be identity) but it's there for completness auto T_a_a = steam::se3::TransformStateEvaluator::MakeShared(stateReference); auto T_a_b = steam::se3::TransformStateEvaluator::MakeShared(stateReading); + auto T_b_a = steam::se3::InverseTransformEvaluator::MakeShared(T_a_b); // Define our error funtion typedef steam::PointToPointErrorEval Error; // Build the alignment errors - Error::Ptr error_0(new Error(ref_a_0, T_a_a, read_b_0, T_a_b)); - Error::Ptr error_1(new Error(ref_a_1, T_a_a, read_b_1, T_a_b)); - Error::Ptr error_2(new Error(ref_a_2, T_a_a, read_b_2, T_a_b)); + Error::Ptr error_0(new Error(ref_a_0, T_a_a, read_b_0, T_b_a)); + Error::Ptr error_1(new Error(ref_a_1, T_a_a, read_b_1, T_b_a)); + Error::Ptr error_2(new Error(ref_a_2, T_a_a, read_b_2, T_b_a)); // Set the NoiseModel (R_i) to identity @@ -155,12 +156,12 @@ void solveSimpleProblem(Eigen::Matrix T_components) "is different than original transformation:" << "\n" << Tgt_b_a.matrix() << "\n" << "difference being:" << "\n" << - stateReading->getValue().matrix() - Tgt_b_a.matrix() + stateReading->getValue().matrix() - Tgt_a_b.matrix() ); // Confirm that our state is the same as our ground truth transformation CHECK(lgmath::common::nearEqual(stateReading->getValue().matrix(), - Tgt_b_a.matrix(), + Tgt_a_b.matrix(), 1e-3 )); From d09aab4c06bc2bd3fc842ea5273712247d1beab1 Mon Sep 17 00:00:00 2001 From: TangTim Date: Wed, 3 Aug 2016 14:38:38 -0400 Subject: [PATCH 08/31] Add files via upload --- include/steam.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/include/steam.hpp b/include/steam.hpp index a0dda44..0d48133 100644 --- a/include/steam.hpp +++ b/include/steam.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include From eca837cb811b29fad994194eb1e0ad777c232dec Mon Sep 17 00:00:00 2001 From: pomerlef Date: Wed, 3 Aug 2016 17:17:48 -0400 Subject: [PATCH 09/31] added new constructor --- .../samples/PointToPointErrorEval.hpp | 72 +++++++++++-------- .../samples/PointToPointErrorEval.cpp | 25 +++++-- 2 files changed, 62 insertions(+), 35 deletions(-) diff --git a/include/steam/evaluator/samples/PointToPointErrorEval.hpp b/include/steam/evaluator/samples/PointToPointErrorEval.hpp index e3a0ffa..ef25a91 100644 --- a/include/steam/evaluator/samples/PointToPointErrorEval.hpp +++ b/include/steam/evaluator/samples/PointToPointErrorEval.hpp @@ -1,3 +1,4 @@ +// vim: ts=4:sw=4:noexpandtab ////////////////////////////////////////////////////////////////////////////////////////////// /// \file PointToPointErrorEval.hpp /// @@ -22,48 +23,61 @@ class PointToPointErrorEval : public ErrorEvaluator<4,6>::type { public: - /// Convenience typedefs - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; + /// Convenience typedefs + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor /// \param ref_a A point from the reference point cloud (static) expressed in homogeneous /// coordinates (i.e., [x, y, z, 1]) and in the frame a. /// \param T_a_world Transformation matrix from frame world to frame a. /// \param read_b A point from the reading point cloud expressed in homogeneous /// coordinates (i.e., [x, y, z, 1]) and in the frame b. /// \param T_b_world Transformation matrix from frame world to frame b. - ////////////////////////////////////////////////////////////////////////////////////////////// - PointToPointErrorEval(const Eigen::Vector4d& ref_a, - const se3::TransformEvaluator::ConstPtr& T_a_world, - const Eigen::Vector4d& read_b, - const se3::TransformEvaluator::ConstPtr& T_b_world - ); + ////////////////////////////////////////////////////////////////////////////////////////////// + PointToPointErrorEval(const Eigen::Vector4d& ref_a, + const se3::TransformEvaluator::ConstPtr& T_a_world, + const Eigen::Vector4d& read_b, + const se3::TransformEvaluator::ConstPtr& T_b_world + ); - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Returns whether or not an evaluator contains unlocked state variables - ////////////////////////////////////////////////////////////////////////////////////////////// - virtual bool isActive() const; //TODO: check if we need to define that + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + /// \param ref_a A point from the reference point cloud (static) expressed in homogeneous + /// coordinates (i.e., [x, y, z, 1]) and in the frame a. + /// \param read_b A point from the reading point cloud expressed in homogeneous + /// coordinates (i.e., [x, y, z, 1]) and in the frame b. + /// \param T_a_b Transformation matrix from frame b to frame a. + ////////////////////////////////////////////////////////////////////////////////////////////// + PointToPointErrorEval(const Eigen::Vector4d& ref_a, + const Eigen::Vector4d& read_b, + const se3::TransformEvaluator::ConstPtr& T_a_b + ); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; //TODO: check if we need to define that - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Evaluate the 4-d measurement error (x, y, z, 0) - ////////////////////////////////////////////////////////////////////////////////////////////// - virtual Eigen::Vector4d evaluate() const; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the 4-d measurement error (x, y, z, 0) + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Vector4d evaluate() const; - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Evaluate the 4-d measurement error (x, y, z, 0) and Jacobians - ////////////////////////////////////////////////////////////////////////////////////////////// - virtual Eigen::Vector4d evaluate(const Eigen::Matrix4d& lhs, - std::vector >* jacs) const; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the 4-d measurement error (x, y, z, 0) and Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Vector4d evaluate(const Eigen::Matrix4d& lhs, + std::vector >* jacs) const; private: - - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Point evaluator (evaluates the point transformed into the camera frame) - ////////////////////////////////////////////////////////////////////////////////////////////// + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Point evaluator (evaluates the point transformed into the camera frame) + ////////////////////////////////////////////////////////////////////////////////////////////// Eigen::Vector4d ref_a_; - se3::ComposeInverseTransformEvaluator::ConstPtr T_ab_; + se3::TransformEvaluator::ConstPtr T_a_b_; Eigen::Vector4d read_b_; }; diff --git a/src/evaluator/samples/PointToPointErrorEval.cpp b/src/evaluator/samples/PointToPointErrorEval.cpp index 9e2c9a8..12bc62e 100644 --- a/src/evaluator/samples/PointToPointErrorEval.cpp +++ b/src/evaluator/samples/PointToPointErrorEval.cpp @@ -21,15 +21,28 @@ PointToPointErrorEval::PointToPointErrorEval( const Eigen::Vector4d& read_b, const se3::TransformEvaluator::ConstPtr& T_b_world ): ref_a_(ref_a), - T_ab_(se3::ComposeInverseTransformEvaluator::MakeShared(T_a_world, T_b_world)), + T_a_b_(se3::ComposeInverseTransformEvaluator::MakeShared(T_a_world, T_b_world)), read_b_(read_b) { } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +PointToPointErrorEval::PointToPointErrorEval( + const Eigen::Vector4d& ref_a, + const Eigen::Vector4d& read_b, + const se3::TransformEvaluator::ConstPtr& T_a_b + ): ref_a_(ref_a), + T_a_b_(T_a_b), + read_b_(read_b){ +} + + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Returns whether or not an evaluator contains unlocked state variables ////////////////////////////////////////////////////////////////////////////////////////////// bool PointToPointErrorEval::isActive() const { - return T_ab_->isActive(); + return T_a_b_->isActive(); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -38,7 +51,7 @@ bool PointToPointErrorEval::isActive() const { Eigen::Vector4d PointToPointErrorEval::evaluate() const { // Return error (between measurement and point estimate projected in camera frame) - return ref_a_ - (T_ab_->evaluate() * read_b_); + return ref_a_ - (T_a_b_->evaluate() * read_b_); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -54,10 +67,10 @@ Eigen::Vector4d PointToPointErrorEval::evaluate(const Eigen::Matrix4d& lhs, std: // Get evaluation tree EvalTreeHandle blkAutoTransform = - T_ab_->getBlockAutomaticEvaluation(); + T_a_b_->getBlockAutomaticEvaluation(); // Get evaluation from tree - //TODO: why just not using T_ab_->evaluate() instead? + //TODO: why just not using T_a_b_->evaluate() instead? const lgmath::se3::Transformation T_ab = blkAutoTransform.getValue(); const Eigen::Vector4d read_a = T_ab * read_b_; @@ -65,7 +78,7 @@ Eigen::Vector4d PointToPointErrorEval::evaluate(const Eigen::Matrix4d& lhs, std: //TODO: why point2fs(...) doesn't take Vector4d as input? //TODO: why is the newLhs not the same shape as lhs (input)? Eigen::Matrix newLhs = -lhs*lgmath::se3::point2fs(read_a.head<3>()); - T_ab_->appendBlockAutomaticJacobians(newLhs, blkAutoTransform.getRoot(), jacs); + T_a_b_->appendBlockAutomaticJacobians(newLhs, blkAutoTransform.getRoot(), jacs); // Return evaluation return ref_a_ - read_a; From 6cfbeea4a4b0b02871670fdd2b2fbf306c48e85c Mon Sep 17 00:00:00 2001 From: pomerlef Date: Wed, 3 Aug 2016 17:20:00 -0400 Subject: [PATCH 10/31] added new constructor --- tests/evaluator_test.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tests/evaluator_test.cpp b/tests/evaluator_test.cpp index 03a8bef..2ae0fd3 100644 --- a/tests/evaluator_test.cpp +++ b/tests/evaluator_test.cpp @@ -23,7 +23,7 @@ Eigen::Vector4d initVector4d(double x, double y, double z) // Helper function to solve and check that the solution is close // to the ground truth transformation used to generate the data -void solveSimpleProblem(Eigen::Matrix T_components) +void solveSimpleProblem_constructor1(Eigen::Matrix T_components) { //--------------------------------------- // General structure: @@ -181,7 +181,7 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { 0.0, // rotation around y-axis 0.0; // rotation around z-axis - solveSimpleProblem(T_components); + solveSimpleProblem_constructor1(T_components); } SECTION("Simple rotation") @@ -193,7 +193,7 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { 0.0, // rotation around y-axis 0.0; // rotation around z-axis - solveSimpleProblem(T_components); + solveSimpleProblem_constructor1(T_components); } SECTION("Random transformation (1000)") @@ -204,7 +204,7 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { { // random numbers in interval [-1, 1] T_components = Eigen::Matrix::Random(); - solveSimpleProblem(T_components); + solveSimpleProblem_constructor1(T_components); } } From 98c180fafe66ea38d95c001bfae2e394ef735398 Mon Sep 17 00:00:00 2001 From: pomerlef Date: Wed, 3 Aug 2016 17:32:39 -0400 Subject: [PATCH 11/31] added new constructor for PointToPointErrorEval --- tests/evaluator_test.cpp | 74 ++++++++++++++++++++++++++++++++++------ 1 file changed, 63 insertions(+), 11 deletions(-) diff --git a/tests/evaluator_test.cpp b/tests/evaluator_test.cpp index 2ae0fd3..44c260c 100644 --- a/tests/evaluator_test.cpp +++ b/tests/evaluator_test.cpp @@ -13,7 +13,7 @@ #include // Helper function to initialize a 3D point in homogeneous coordinates -Eigen::Vector4d initVector4d(double x, double y, double z) +Eigen::Vector4d initVector4d(const double x, const double y, const double z) { Eigen::Vector4d v; v << x, y, z, 1.0; @@ -23,7 +23,7 @@ Eigen::Vector4d initVector4d(double x, double y, double z) // Helper function to solve and check that the solution is close // to the ground truth transformation used to generate the data -void solveSimpleProblem_constructor1(Eigen::Matrix T_components) +void solveSimpleProblem(const Eigen::Matrix T_components, const int constructor_type) { //--------------------------------------- // General structure: @@ -88,11 +88,27 @@ void solveSimpleProblem_constructor1(Eigen::Matrix T_components) // Define our error funtion typedef steam::PointToPointErrorEval Error; + //------------- + // This is specific to the unit test + // Build the alignment errors - Error::Ptr error_0(new Error(ref_a_0, T_a_a, read_b_0, T_b_a)); - Error::Ptr error_1(new Error(ref_a_1, T_a_a, read_b_1, T_b_a)); - Error::Ptr error_2(new Error(ref_a_2, T_a_a, read_b_2, T_b_a)); + Error::Ptr error_0; + Error::Ptr error_1; + Error::Ptr error_2; + if(constructor_type == 0) + { + error_0.reset(new Error(ref_a_0, T_a_a, read_b_0, T_b_a)); + error_1.reset(new Error(ref_a_1, T_a_a, read_b_1, T_b_a)); + error_2.reset(new Error(ref_a_2, T_a_a, read_b_2, T_b_a)); + } + else if(constructor_type == 1) + { + error_0.reset(new Error(ref_a_0, read_b_0, T_a_b)); + error_1.reset(new Error(ref_a_1, read_b_1, T_a_b)); + error_2.reset(new Error(ref_a_2, read_b_2, T_a_b)); + } + //------------- // Set the NoiseModel (R_i) to identity steam::BaseNoiseModel<4>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<4>(Eigen::Matrix4d::Identity())); @@ -172,7 +188,7 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { Eigen::Matrix T_components; - SECTION("Simple translation") + SECTION("Simple translation - constructor 0") { T_components << 1.0, // translation x 0.0, // translation y @@ -181,10 +197,10 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { 0.0, // rotation around y-axis 0.0; // rotation around z-axis - solveSimpleProblem_constructor1(T_components); + solveSimpleProblem(T_components, 0); } - SECTION("Simple rotation") + SECTION("Simple rotation - constructor 0") { T_components << 0.0, // translation x 0.0, // translation y @@ -193,10 +209,10 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { 0.0, // rotation around y-axis 0.0; // rotation around z-axis - solveSimpleProblem_constructor1(T_components); + solveSimpleProblem(T_components, 0); } - SECTION("Random transformation (1000)") + SECTION("Random transformation (1000) - constructor 0") { srand((unsigned int) time(0)); @@ -204,10 +220,46 @@ TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { { // random numbers in interval [-1, 1] T_components = Eigen::Matrix::Random(); - solveSimpleProblem_constructor1(T_components); + solveSimpleProblem(T_components, 0); } } + + SECTION("Simple translation - constructor 1") + { + T_components << 1.0, // translation x + 0.0, // translation y + 0.0, // translation z + 0.0, // rotation around x-axis + 0.0, // rotation around y-axis + 0.0; // rotation around z-axis + solveSimpleProblem(T_components, 1); + } + + SECTION("Simple rotation - constructor 1") + { + T_components << 0.0, // translation x + 0.0, // translation y + 0.0, // translation z + 1.0, // rotation around x-axis + 0.0, // rotation around y-axis + 0.0; // rotation around z-axis + + solveSimpleProblem(T_components, 1); + } + + SECTION("Random transformation (1000) - constructor 1") + { + srand((unsigned int) time(0)); + + for(int i=0; i<1000; i++) + { + // random numbers in interval [-1, 1] + T_components = Eigen::Matrix::Random(); + solveSimpleProblem(T_components, 1); + } + + } } // TEST_CASE From 34bd42fcc209fd8a4a7c788926628f3160c29315 Mon Sep 17 00:00:00 2001 From: TangTim Date: Mon, 17 Oct 2016 16:32:10 -0400 Subject: [PATCH 12/31] Fixed the transformation in PointToPointErrorEval to be the correct direction --- .../samples/PointToPointErrorEval.hpp | 4 +-- .../samples/PointToPointErrorEval.cpp | 33 +++++++++++-------- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/include/steam/evaluator/samples/PointToPointErrorEval.hpp b/include/steam/evaluator/samples/PointToPointErrorEval.hpp index ef25a91..c5e0279 100644 --- a/include/steam/evaluator/samples/PointToPointErrorEval.hpp +++ b/include/steam/evaluator/samples/PointToPointErrorEval.hpp @@ -52,7 +52,7 @@ class PointToPointErrorEval : public ErrorEvaluator<4,6>::type ////////////////////////////////////////////////////////////////////////////////////////////// PointToPointErrorEval(const Eigen::Vector4d& ref_a, const Eigen::Vector4d& read_b, - const se3::TransformEvaluator::ConstPtr& T_a_b + const se3::TransformEvaluator::ConstPtr& T_b_a ); ////////////////////////////////////////////////////////////////////////////////////////////// @@ -77,7 +77,7 @@ class PointToPointErrorEval : public ErrorEvaluator<4,6>::type /// \brief Point evaluator (evaluates the point transformed into the camera frame) ////////////////////////////////////////////////////////////////////////////////////////////// Eigen::Vector4d ref_a_; - se3::TransformEvaluator::ConstPtr T_a_b_; + se3::TransformEvaluator::ConstPtr T_b_a_; Eigen::Vector4d read_b_; }; diff --git a/src/evaluator/samples/PointToPointErrorEval.cpp b/src/evaluator/samples/PointToPointErrorEval.cpp index 12bc62e..bb77c16 100644 --- a/src/evaluator/samples/PointToPointErrorEval.cpp +++ b/src/evaluator/samples/PointToPointErrorEval.cpp @@ -21,7 +21,7 @@ PointToPointErrorEval::PointToPointErrorEval( const Eigen::Vector4d& read_b, const se3::TransformEvaluator::ConstPtr& T_b_world ): ref_a_(ref_a), - T_a_b_(se3::ComposeInverseTransformEvaluator::MakeShared(T_a_world, T_b_world)), + T_b_a_(se3::ComposeInverseTransformEvaluator::MakeShared(T_a_world, T_b_world)), read_b_(read_b) { } @@ -31,9 +31,9 @@ PointToPointErrorEval::PointToPointErrorEval( PointToPointErrorEval::PointToPointErrorEval( const Eigen::Vector4d& ref_a, const Eigen::Vector4d& read_b, - const se3::TransformEvaluator::ConstPtr& T_a_b + const se3::TransformEvaluator::ConstPtr& T_b_a ): ref_a_(ref_a), - T_a_b_(T_a_b), + T_b_a_(T_b_a), read_b_(read_b){ } @@ -42,7 +42,7 @@ PointToPointErrorEval::PointToPointErrorEval( /// \brief Returns whether or not an evaluator contains unlocked state variables ////////////////////////////////////////////////////////////////////////////////////////////// bool PointToPointErrorEval::isActive() const { - return T_a_b_->isActive(); + return T_b_a_->isActive(); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -51,7 +51,8 @@ bool PointToPointErrorEval::isActive() const { Eigen::Vector4d PointToPointErrorEval::evaluate() const { // Return error (between measurement and point estimate projected in camera frame) - return ref_a_ - (T_a_b_->evaluate() * read_b_); + // return ref_a_ - (T_b_a_->evaluate().inverse() * read_b_); + return read_b_ - T_b_a_->evaluate() * ref_a_; } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -66,22 +67,28 @@ Eigen::Vector4d PointToPointErrorEval::evaluate(const Eigen::Matrix4d& lhs, std: jacs->clear(); // Get evaluation tree + EvalTreeHandle blkAutoTransform = - T_a_b_->getBlockAutomaticEvaluation(); + T_b_a_->getBlockAutomaticEvaluation(); // Get evaluation from tree //TODO: why just not using T_a_b_->evaluate() instead? - const lgmath::se3::Transformation T_ab = blkAutoTransform.getValue(); - const Eigen::Vector4d read_a = T_ab * read_b_; + const lgmath::se3::Transformation T_ba = blkAutoTransform.getValue(); + // const lgmath::se3::Transformation T_ba = T_b_a_->evaluate(); + // const Eigen::Vector4d read_a = T_ba.inverse() * read_b_; + const Eigen::Vector4d ref_b = T_ba * ref_a_; // Get Jacobians - //TODO: why point2fs(...) doesn't take Vector4d as input? - //TODO: why is the newLhs not the same shape as lhs (input)? - Eigen::Matrix newLhs = -lhs*lgmath::se3::point2fs(read_a.head<3>()); - T_a_b_->appendBlockAutomaticJacobians(newLhs, blkAutoTransform.getRoot(), jacs); + + // Eigen::Matrix newLhs = -lhs*lgmath::se3::point2fs(read_a.head<3>()); + // Eigen::Matrix newLhs = lhs*T_ba.adjoint()*lgmath::se3::point2fs(read_a.head<3>()); + Eigen::Matrix newLhs = -lhs*lgmath::se3::point2fs(ref_b.head<3>()); + + T_b_a_->appendBlockAutomaticJacobians(newLhs, blkAutoTransform.getRoot(), jacs); // Return evaluation - return ref_a_ - read_a; + // return ref_a_ - read_a; + return read_b_ - ref_b; } From 8a30fc9081189cd5690bf04f1253189b05dca169 Mon Sep 17 00:00:00 2001 From: TangTim Date: Wed, 18 Jul 2018 16:41:32 -0500 Subject: [PATCH 13/31] Defined trajectory state variable that includes acceleration --- include/steam/trajectory_ca/SteamTrajVar.hpp | 84 ++++++ samples/CMakeLists.txt | 4 + samples/SimpleBAandCATrajPrior.cpp | 276 +++++++++++++++++++ src/trajectory_ca/SteamTrajVar.cpp | 63 +++++ 4 files changed, 427 insertions(+) create mode 100644 include/steam/trajectory_ca/SteamTrajVar.hpp create mode 100644 samples/SimpleBAandCATrajPrior.cpp create mode 100644 src/trajectory_ca/SteamTrajVar.cpp diff --git a/include/steam/trajectory_ca/SteamTrajVar.hpp b/include/steam/trajectory_ca/SteamTrajVar.hpp new file mode 100644 index 0000000..c952796 --- /dev/null +++ b/include/steam/trajectory_ca/SteamTrajVar.hpp @@ -0,0 +1,84 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajVar.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_TRAJECTORY_VAR_HPP +#define STEAM_TRAJECTORY_VAR_HPP + +#include + +#include +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief This class wraps a pose and velocity evaluator to act as a discrete-time trajectory +/// state variable for continuous-time trajectory estimation. +////////////////////////////////////////////////////////////////////////////////////////////// +class SteamCATrajVar +{ + public: + + /// Shared pointer typedefs for readability + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get pose evaluator + ////////////////////////////////////////////////////////////////////////////////////////////// + const se3::TransformEvaluator::Ptr& getPose() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get velocity state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + const VectorSpaceStateVar::Ptr& getVelocity() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get velocity state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + const VectorSpaceStateVar::Ptr& getAcceleration() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get timestamp + ////////////////////////////////////////////////////////////////////////////////////////////// + const steam::Time& getTime() const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Timestamp of trajectory variable + ////////////////////////////////////////////////////////////////////////////////////////////// + steam::Time time_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Pose evaluator + ////////////////////////////////////////////////////////////////////////////////////////////// + se3::TransformEvaluator::Ptr T_k0_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Generalized 6D velocity state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::Ptr velocity_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Generalized 6D acceleration state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::Ptr acceleration_; +}; + +} // se3 +} // steam + +#endif // STEAM_TRAJECTORY_VAR_HPP diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 2943546..8c82045 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -40,3 +40,7 @@ target_link_libraries(SimpleTrajectoryPrior steam ${DEPEND_LIBS}) # simple example of trajectory interface for prior, combined with bundle adjustment add_executable(SimpleBAandTrajPrior ${CMAKE_CURRENT_SOURCE_DIR}/SimpleBAandTrajPrior.cpp) target_link_libraries(SimpleBAandTrajPrior steam ${DEPEND_LIBS}) + +# simple example of constant acceleration trajectory interface for prior, combined with bundle adjustment +add_executable(SimpleBAandCATrajPrior ${CMAKE_CURRENT_SOURCE_DIR}/SimpleBAandCATrajPrior.cpp) +target_link_libraries(SimpleBAandCATrajPrior steam ${DEPEND_LIBS}) diff --git a/samples/SimpleBAandCATrajPrior.cpp b/samples/SimpleBAandCATrajPrior.cpp new file mode 100644 index 0000000..299302e --- /dev/null +++ b/samples/SimpleBAandCATrajPrior.cpp @@ -0,0 +1,276 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SimpleBAandTrajPrior.cpp +/// \brief A sample usage of the STEAM Engine library for a bundle adjustment problem +/// with relative landmarks and trajectory smoothing factors. +/// +/// \author Sean Anderson, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Structure to store trajectory state variables +////////////////////////////////////////////////////////////////////////////////////////////// +struct TrajStateVar { + steam::Time time; + steam::se3::TransformStateVar::Ptr pose; + steam::VectorSpaceStateVar::Ptr velocity; +}; + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Example that loads and solves simple bundle adjustment problems +////////////////////////////////////////////////////////////////////////////////////////////// +int main(int argc, char** argv) { + + /// + /// Setup Traj Prior + /// + + + // Smoothing factor diagonal -- in this example, we penalize accelerations in each dimension + // except for the forward and yaw (this should be fairly typical) + double lin_acc_stddev_x = 1.00; // body-centric (e.g. x is usually forward) + double lin_acc_stddev_y = 0.01; // body-centric (e.g. y is usually side-slip) + double lin_acc_stddev_z = 0.01; // body-centric (e.g. z is usually 'jump') + double ang_acc_stddev_x = 0.01; // ~roll + double ang_acc_stddev_y = 0.01; // ~pitch + double ang_acc_stddev_z = 1.00; // ~yaw + Eigen::Array Qc_diag; + Qc_diag << lin_acc_stddev_x, lin_acc_stddev_y, lin_acc_stddev_z, + ang_acc_stddev_x, ang_acc_stddev_y, ang_acc_stddev_z; + + // Make Qc_inv + Eigen::Matrix Qc_inv; Qc_inv.setZero(); + Qc_inv.diagonal() = 1.0/Qc_diag; + + /// + /// Parse Dataset + /// + + // Get filename + std::string filename; + if (argc < 2) { + filename = "../../include/steam/data/stereo_simulated.txt"; + //filename = "../../include/steam/data/stereo_simulated_window1.txt"; + //filename = "../../include/steam/data/stereo_simulated_window2.txt"; + std::cout << "Parsing default file: " << filename << std::endl << std::endl; + } else { + filename = argv[1]; + std::cout << "Parsing file: " << filename << std::endl << std::endl; + } + + // Load dataset + steam::data::SimpleBaDataset dataset = steam::data::parseSimpleBaDataset(filename); + std::cout << "Problem has: " << dataset.frames_gt.size() << " poses" << std::endl; + std::cout << " " << dataset.frames_gt.size() << " velocities" << std::endl; + std::cout << " " << dataset.land_gt.size() << " landmarks" << std::endl; + std::cout << " ~" << double(dataset.meas.size())/dataset.frames_gt.size() << " meas per pose" << std::endl << std::endl; + + /// + /// Setup States + /// + + // Set a fixed identity transform that will be used to initialize landmarks in their parent frame + steam::se3::FixedTransformEvaluator::Ptr tf_identity = + steam::se3::FixedTransformEvaluator::MakeShared(lgmath::se3::Transformation()); + + // Fixed vehicle to camera transform + steam::se3::TransformEvaluator::Ptr tf_c_v = + steam::se3::FixedTransformEvaluator::MakeShared(dataset.T_cv); + + // Ground truth + std::vector poses_gt_k_0; + std::vector landmarks_gt; + + // State variable containers (and related data) + std::vector traj_states_ic; + //std::vector poses_ic_k_0; + std::vector landmarks_ic; + + // Record the frame in which the landmark is first seen in order to set up transforms correctly + std::map landmark_map; + + /// + /// Initialize States + /// + + // Setup ground-truth poses + for (unsigned int i = 0; i < dataset.frames_gt.size(); i++) { + steam::se3::TransformStateVar::Ptr temp(new steam::se3::TransformStateVar(dataset.frames_gt[i].T_k0)); + poses_gt_k_0.push_back(temp); + } + + // Setup ground-truth landmarks + for (unsigned int i = 0; i < dataset.land_gt.size(); i++) { + steam::se3::LandmarkStateVar::Ptr temp(new steam::se3::LandmarkStateVar(dataset.land_gt[i].point)); + landmarks_gt.push_back(temp); + } + + // Zero velocity + Eigen::Matrix initVelocity; initVelocity.setZero(); + + // Setup state variables using initial condition + for (unsigned int i = 0; i < dataset.frames_ic.size(); i++) { + TrajStateVar temp; + temp.time = steam::Time(dataset.frames_ic[i].time); + temp.pose = steam::se3::TransformStateVar::Ptr(new steam::se3::TransformStateVar(dataset.frames_ic[i].T_k0)); + temp.velocity = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initVelocity)); + std::cout << i << " : " << dataset.frames_ic[i].time << " " << dataset.frames_ic[i].T_k0; + traj_states_ic.push_back(temp); + } + + // Setup Trajectory + steam::se3::SteamTrajInterface traj(Qc_inv); + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + TrajStateVar& state = traj_states_ic.at(i); + steam::se3::TransformStateEvaluator::Ptr temp = + steam::se3::TransformStateEvaluator::MakeShared(state.pose); + traj.add(state.time, temp, state.velocity); + } + + // Lock first pose (otherwise entire solution is 'floating') + // **Note: alternatively we could add a prior to the first pose. + traj_states_ic[0].pose->setLock(true); + + // Setup relative landmarks + landmarks_ic.resize(dataset.land_ic.size()); + for (unsigned int i = 0; i < dataset.meas.size(); i++) { + + // Get pose reference + unsigned int frameIdx = dataset.meas[i].frameID; + + // Get landmark reference + unsigned int landmarkIdx = dataset.meas[i].landID; + + // Setup landmark if first time + if (!landmarks_ic[landmarkIdx]) { + + // Get homogeneous point in inertial frame + Eigen::Vector4d p_0; + p_0.head<3>() = dataset.land_ic[landmarkIdx].point; + p_0[3] = 1.0; + + // Get transform between first observation time and inertial frame + lgmath::se3::Transformation pose_vk_0 = dataset.frames_ic[frameIdx].T_k0/dataset.frames_ic[0].T_k0; + + // Get point in 'local' frame + Eigen::Vector4d p_vehicle = pose_vk_0 * p_0; + + // Insert the landmark + landmarks_ic[landmarkIdx] = steam::se3::LandmarkStateVar::Ptr(new steam::se3::LandmarkStateVar(p_vehicle.head<3>())); + + // Keep a record of its 'parent' frame + landmark_map[landmarkIdx] = frameIdx; + } + } + + /// + /// Setup Cost Terms + /// + + // steam cost terms + steam::ParallelizedCostTermCollection::Ptr stereoCostTerms(new steam::ParallelizedCostTermCollection()); + + // Setup shared noise and loss function + steam::BaseNoiseModel<4>::Ptr sharedCameraNoiseModel(new steam::StaticNoiseModel<4>(dataset.noise)); + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + + // Setup camera intrinsics + steam::stereo::CameraIntrinsics::Ptr sharedIntrinsics( + new steam::stereo::CameraIntrinsics()); + sharedIntrinsics->b = dataset.camParams.b; + sharedIntrinsics->fu = dataset.camParams.fu; + sharedIntrinsics->fv = dataset.camParams.fv; + sharedIntrinsics->cu = dataset.camParams.cu; + sharedIntrinsics->cv = dataset.camParams.cv; + + // Generate cost terms for camera measurements + for (unsigned int i = 0; i < dataset.meas.size(); i++) { + + // Get pose reference + unsigned int frameIdx = dataset.meas[i].frameID; + + // Get landmark reference + unsigned int landmarkIdx = dataset.meas[i].landID; + steam::se3::LandmarkStateVar::Ptr& landVar = landmarks_ic[landmarkIdx]; + + // Construct transform evaluator between two vehicle frames (a and b) that have observations + steam::se3::TransformEvaluator::Ptr tf_vb_va; + if(landmark_map[landmarkIdx] == frameIdx) { + + // In this case, the transform remains fixed as an identity transform + tf_vb_va = tf_identity; + + } else { + + unsigned int firstObsIndex = landmark_map[landmarkIdx]; + steam::se3::TransformEvaluator::Ptr pose_va_0 = steam::se3::TransformStateEvaluator::MakeShared(traj_states_ic[firstObsIndex].pose); + steam::se3::TransformEvaluator::Ptr pose_vb_0 = steam::se3::TransformStateEvaluator::MakeShared(traj_states_ic[frameIdx].pose); + tf_vb_va = steam::se3::composeInverse(pose_vb_0, pose_va_0); + } + + // Compose with camera to vehicle transform + steam::se3::TransformEvaluator::Ptr tf_cb_va = steam::se3::compose(tf_c_v, tf_vb_va); + + // Construct error function + steam::StereoCameraErrorEval::Ptr errorfunc(new steam::StereoCameraErrorEval( + dataset.meas[i].data, sharedIntrinsics, tf_cb_va, landVar)); + + // Construct cost term + steam::WeightedLeastSqCostTerm<4,6>::Ptr cost(new steam::WeightedLeastSqCostTerm<4,6>(errorfunc, sharedCameraNoiseModel, sharedLossFunc)); + stereoCostTerms->add(cost); + } + + // Trajectory prior smoothing terms + steam::ParallelizedCostTermCollection::Ptr smoothingCostTerms(new steam::ParallelizedCostTermCollection()); + traj.appendPriorCostTerms(smoothingCostTerms); + + /// + /// Make Optimization Problem + /// + + // Initialize problem + steam::OptimizationProblem problem; + + // Add state variables + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + const TrajStateVar& state = traj_states_ic.at(i); + problem.addStateVariable(state.pose); + problem.addStateVariable(state.velocity); + } + + // Add landmark variables + for (unsigned int i = 0; i < landmarks_ic.size(); i++) { + problem.addStateVariable(landmarks_ic[i]); + } + + // Add cost terms + problem.addCostTerm(stereoCostTerms); + problem.addCostTerm(smoothingCostTerms); + + /// + /// Setup Solver and Optimize + /// + typedef steam::DoglegGaussNewtonSolver SolverType; + + // Initialize parameters (enable verbose mode) + SolverType::Params params; + params.verbose = true; + + // Make solver + SolverType solver(&problem, params); + + // Optimize + solver.optimize(); + + // Setup Trajectory + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + TrajStateVar& state = traj_states_ic.at(i); + // std::cout << i << ": \n " << state.velocity->getValue() << "\n"; + } + return 0; +} diff --git a/src/trajectory_ca/SteamTrajVar.cpp b/src/trajectory_ca/SteamTrajVar.cpp new file mode 100644 index 0000000..24d50e6 --- /dev/null +++ b/src/trajectory_ca/SteamTrajVar.cpp @@ -0,0 +1,63 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajVar.cpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +SteamCATrajVar::SteamCATrajVar(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration) + : time_(time), T_k0_(T_k0), velocity_(velocity), acceleration_(acceleration) { + + // Check velocity input + if (velocity->getPerturbDim() != 6) { + throw std::invalid_argument("invalid velocity size"); + } + + // Check acceleration input + if (acceleration->getPerturbDim() != 6) { + throw std::invalid_argument("invalid acceleration size"); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get pose evaluator +////////////////////////////////////////////////////////////////////////////////////////////// +const se3::TransformEvaluator::Ptr& SteamCATrajVar::getPose() const { + return T_k0_; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get velocity state variable +////////////////////////////////////////////////////////////////////////////////////////////// +const VectorSpaceStateVar::Ptr& SteamCATrajVar::getVelocity() const { + return velocity_; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get acceleration state variable +////////////////////////////////////////////////////////////////////////////////////////////// +const VectorSpaceStateVar::Ptr& SteamCATrajVar::getAcceleration() const { + return acceleration_; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get timestamp +////////////////////////////////////////////////////////////////////////////////////////////// +const steam::Time& SteamCATrajVar::getTime() const { + return time_; +} + +} // se3 +} // steam From 8f57d32a3dce6ff3906458e657716b3939e3ecc8 Mon Sep 17 00:00:00 2001 From: TangTim Date: Wed, 18 Jul 2018 18:20:59 -0500 Subject: [PATCH 14/31] Implemented trajectory error function and jacobians for CA prior. Can compile but cannot run sample problem yet --- include/steam.hpp | 1 + .../trajectory_ca/SteamCATrajInterface.hpp | 123 +++++++ .../trajectory_ca/SteamCATrajPriorFactor.hpp | 67 ++++ .../{SteamTrajVar.hpp => SteamCATrajVar.hpp} | 6 +- samples/SimpleBAandCATrajPrior.cpp | 21 +- samples/SimpleBAandTrajPrior.cpp | 2 +- src/trajectory_ca/SteamCATrajInterface.cpp | 303 ++++++++++++++++++ src/trajectory_ca/SteamCATrajPriorFactor.cpp | 189 +++++++++++ .../{SteamTrajVar.cpp => SteamCATrajVar.cpp} | 2 +- 9 files changed, 703 insertions(+), 11 deletions(-) create mode 100644 include/steam/trajectory_ca/SteamCATrajInterface.hpp create mode 100644 include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp rename include/steam/trajectory_ca/{SteamTrajVar.hpp => SteamCATrajVar.hpp} (97%) create mode 100644 src/trajectory_ca/SteamCATrajInterface.cpp create mode 100644 src/trajectory_ca/SteamCATrajPriorFactor.cpp rename src/trajectory_ca/{SteamTrajVar.cpp => SteamCATrajVar.cpp} (97%) diff --git a/include/steam.hpp b/include/steam.hpp index 0d48133..97fbd77 100644 --- a/include/steam.hpp +++ b/include/steam.hpp @@ -61,5 +61,6 @@ // trajectory #include +#include #endif // STEAM_ESTIMATION_LIBRARY_HPP diff --git a/include/steam/trajectory_ca/SteamCATrajInterface.hpp b/include/steam/trajectory_ca/SteamCATrajInterface.hpp new file mode 100644 index 0000000..4edd867 --- /dev/null +++ b/include/steam/trajectory_ca/SteamCATrajInterface.hpp @@ -0,0 +1,123 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajInterface.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CA_TRAJECTORY_INTERFACE_HPP +#define STEAM_CA_TRAJECTORY_INTERFACE_HPP + +#include + +#include + +#include + +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief The trajectory class wraps a set of state variables to provide an interface +/// that allows for continuous-time pose interpolation. +////////////////////////////////////////////////////////////////////////////////////////////// +class SteamCATrajInterface +{ + public: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + /// Note that the weighting matrix, Qc, should be provided if prior factors are needed + /// for estimation. Without Qc the interpolation methods can be used safely. + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajInterface(bool allowExtrapolation = false); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajInterface(const Eigen::Matrix& Qc_inv, bool allowExtrapolation = false); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Add a new knot + ////////////////////////////////////////////////////////////////////////////////////////////// + void add(const SteamCATrajVar::Ptr& knot); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Add a new knot + ////////////////////////////////////////////////////////////////////////////////////////////// + void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get evaluator + ////////////////////////////////////////////////////////////////////////////////////////////// + TransformEvaluator::ConstPtr getInterpPoseEval(const steam::Time& time) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior + /// should exist on a trajectory, adding a second will overwrite the first. + ////////////////////////////////////////////////////////////////////////////////////////////// + void addPosePrior(const steam::Time& time, const lgmath::se3::Transformation& pose, + const Eigen::Matrix& cov); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Add a unary velocity prior factor at a knot time. Note that only a single velocity + /// prior should exist on a trajectory, adding a second will overwrite the first. + ////////////////////////////////////////////////////////////////////////////////////////////// + void addVelocityPrior(const steam::Time& time, const Eigen::Matrix& velocity, + const Eigen::Matrix& cov); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Add a unary acceleration prior factor at a knot time. Note that only a single acceleration + /// prior should exist on a trajectory, adding a second will overwrite the first. + ////////////////////////////////////////////////////////////////////////////////////////////// + void addAccelerationPrior(const steam::Time& time, const Eigen::Matrix& acceleration, + const Eigen::Matrix& cov); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get binary cost terms associated with the prior for active parts of the trajectory + ////////////////////////////////////////////////////////////////////////////////////////////// + void appendPriorCostTerms(const ParallelizedCostTermCollection::Ptr& costTerms) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get active state variables in the trajectory + ////////////////////////////////////////////////////////////////////////////////////////////// + void getActiveStateVariables( + std::map* outStates) const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Ordered map of knots + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::Matrix Qc_inv_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Allow for extrapolation + ////////////////////////////////////////////////////////////////////////////////////////////// + bool allowExtrapolation_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Pose prior + ////////////////////////////////////////////////////////////////////////////////////////////// + steam::WeightedLeastSqCostTerm<6,6>::Ptr posePriorFactor_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Velocity prior + ////////////////////////////////////////////////////////////////////////////////////////////// + steam::WeightedLeastSqCostTerm<6,6>::Ptr velocityPriorFactor_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Ordered map of knots + ////////////////////////////////////////////////////////////////////////////////////////////// + std::map knotMap_; + +}; + +} // se3 +} // steam + +#endif // STEAM_CA_TRAJECTORY_INTERFACE_HPP diff --git a/include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp b/include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp new file mode 100644 index 0000000..3e989a8 --- /dev/null +++ b/include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp @@ -0,0 +1,67 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajPriorFactor.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CA_TRAJECTORY_PRIOR_FACTOR_HPP +#define STEAM_CA_TRAJECTORY_PRIOR_FACTOR_HPP + +#include + +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Gaussian-process prior evaluator +////////////////////////////////////////////////////////////////////////////////////////////// +class SteamCATrajPriorFactor : public ErrorEvaluatorX +{ + public: + + /// Shared pointer typedefs for readability + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajPriorFactor(const SteamCATrajVar::ConstPtr& knot1, + const SteamCATrajVar::ConstPtr& knot2); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the GP prior factor + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::VectorXd evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the GP prior factor and Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::VectorXd evaluate(const Eigen::MatrixXd& lhs, std::vector >* jacs) const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief First (earlier) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajVar::ConstPtr knot1_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Second (later) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajVar::ConstPtr knot2_; +}; + +} // se3 +} // steam + +#endif // STEAM_CA_TRAJECTORY_PRIOR_FACTOR_HPP + diff --git a/include/steam/trajectory_ca/SteamTrajVar.hpp b/include/steam/trajectory_ca/SteamCATrajVar.hpp similarity index 97% rename from include/steam/trajectory_ca/SteamTrajVar.hpp rename to include/steam/trajectory_ca/SteamCATrajVar.hpp index c952796..72162cb 100644 --- a/include/steam/trajectory_ca/SteamTrajVar.hpp +++ b/include/steam/trajectory_ca/SteamCATrajVar.hpp @@ -4,8 +4,8 @@ /// \author Tim Tang, ASRL ////////////////////////////////////////////////////////////////////////////////////////////// -#ifndef STEAM_TRAJECTORY_VAR_HPP -#define STEAM_TRAJECTORY_VAR_HPP +#ifndef STEAM_CA_TRAJECTORY_VAR_HPP +#define STEAM_CA_TRAJECTORY_VAR_HPP #include @@ -81,4 +81,4 @@ class SteamCATrajVar } // se3 } // steam -#endif // STEAM_TRAJECTORY_VAR_HPP +#endif // STEAM_CA_TRAJECTORY_VAR_HPP diff --git a/samples/SimpleBAandCATrajPrior.cpp b/samples/SimpleBAandCATrajPrior.cpp index 299302e..b568292 100644 --- a/samples/SimpleBAandCATrajPrior.cpp +++ b/samples/SimpleBAandCATrajPrior.cpp @@ -1,9 +1,9 @@ ////////////////////////////////////////////////////////////////////////////////////////////// -/// \file SimpleBAandTrajPrior.cpp +/// \file SimpleBAandCATrajPrior.cpp /// \brief A sample usage of the STEAM Engine library for a bundle adjustment problem /// with relative landmarks and trajectory smoothing factors. /// -/// \author Sean Anderson, ASRL +/// \author Tim Tang, ASRL ////////////////////////////////////////////////////////////////////////////////////////////// #include @@ -19,6 +19,7 @@ struct TrajStateVar { steam::Time time; steam::se3::TransformStateVar::Ptr pose; steam::VectorSpaceStateVar::Ptr velocity; + steam::VectorSpaceStateVar::Ptr acceleration; }; ////////////////////////////////////////////////////////////////////////////////////////////// @@ -113,29 +114,36 @@ int main(int argc, char** argv) { // Zero velocity Eigen::Matrix initVelocity; initVelocity.setZero(); + // Zero acceleration + Eigen::Matrix initAcceleration; initAcceleration.setZero(); + // Setup state variables using initial condition for (unsigned int i = 0; i < dataset.frames_ic.size(); i++) { TrajStateVar temp; temp.time = steam::Time(dataset.frames_ic[i].time); temp.pose = steam::se3::TransformStateVar::Ptr(new steam::se3::TransformStateVar(dataset.frames_ic[i].T_k0)); temp.velocity = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initVelocity)); - std::cout << i << " : " << dataset.frames_ic[i].time << " " << dataset.frames_ic[i].T_k0; + temp.acceleration = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initAcceleration)); + // std::cout << i << " : " << dataset.frames_ic[i].time << " " << dataset.frames_ic[i].T_k0; traj_states_ic.push_back(temp); } // Setup Trajectory - steam::se3::SteamTrajInterface traj(Qc_inv); + steam::se3::SteamCATrajInterface traj(Qc_inv); for (unsigned int i = 0; i < traj_states_ic.size(); i++) { TrajStateVar& state = traj_states_ic.at(i); steam::se3::TransformStateEvaluator::Ptr temp = steam::se3::TransformStateEvaluator::MakeShared(state.pose); - traj.add(state.time, temp, state.velocity); + traj.add(state.time, temp, state.velocity, state.acceleration); } // Lock first pose (otherwise entire solution is 'floating') // **Note: alternatively we could add a prior to the first pose. traj_states_ic[0].pose->setLock(true); + // Lock the first velocity as well + traj_states_ic[0].velocity->setLock(true); + // Setup relative landmarks landmarks_ic.resize(dataset.land_ic.size()); for (unsigned int i = 0; i < dataset.meas.size(); i++) { @@ -241,6 +249,7 @@ int main(int argc, char** argv) { const TrajStateVar& state = traj_states_ic.at(i); problem.addStateVariable(state.pose); problem.addStateVariable(state.velocity); + // problem.addStateVariable(state.acceleration); } // Add landmark variables @@ -270,7 +279,7 @@ int main(int argc, char** argv) { // Setup Trajectory for (unsigned int i = 0; i < traj_states_ic.size(); i++) { TrajStateVar& state = traj_states_ic.at(i); - // std::cout << i << ": \n " << state.velocity->getValue() << "\n"; + std::cout << i << ": \n " << state.pose->getValue() << "\n"; } return 0; } diff --git a/samples/SimpleBAandTrajPrior.cpp b/samples/SimpleBAandTrajPrior.cpp index 299302e..550a5c5 100644 --- a/samples/SimpleBAandTrajPrior.cpp +++ b/samples/SimpleBAandTrajPrior.cpp @@ -270,7 +270,7 @@ int main(int argc, char** argv) { // Setup Trajectory for (unsigned int i = 0; i < traj_states_ic.size(); i++) { TrajStateVar& state = traj_states_ic.at(i); - // std::cout << i << ": \n " << state.velocity->getValue() << "\n"; + std::cout << i << ": \n " << state.pose->getValue() << "\n"; } return 0; } diff --git a/src/trajectory_ca/SteamCATrajInterface.cpp b/src/trajectory_ca/SteamCATrajInterface.cpp new file mode 100644 index 0000000..79315ed --- /dev/null +++ b/src/trajectory_ca/SteamCATrajInterface.cpp @@ -0,0 +1,303 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajInterface.cpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +// #include +#include +#include + +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +/// Note, without providing Qc, the trajectory can be used safely for interpolation, +/// but should not be used for estimation. +////////////////////////////////////////////////////////////////////////////////////////////// +SteamCATrajInterface::SteamCATrajInterface(bool allowExtrapolation) : + Qc_inv_(Eigen::Matrix::Identity()), allowExtrapolation_(allowExtrapolation) { +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +SteamCATrajInterface::SteamCATrajInterface(const Eigen::Matrix& Qc_inv, + bool allowExtrapolation) : + Qc_inv_(Qc_inv), allowExtrapolation_(allowExtrapolation) { +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Add a new knot +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajInterface::add(const SteamCATrajVar::Ptr& knot) { + + // Todo, check that time does not already exist in map? + + // Insert in map + knotMap_.insert(knotMap_.end(), + std::pair(knot->getTime().nanosecs(), knot)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Add a new knot +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajInterface::add(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration) { + + // Check velocity input + if (velocity->getPerturbDim() != 6) { + throw std::invalid_argument("invalid velocity size"); + } + + // Check acceleration input + if (acceleration->getPerturbDim() != 6) { + throw std::invalid_argument("invalid acceleration size"); + } + + // Todo, check that time does not already exist in map? + + // Make knot + SteamCATrajVar::Ptr newEntry(new SteamCATrajVar(time, T_k0, velocity, acceleration)); + + // Insert in map + knotMap_.insert(knotMap_.end(), + std::pair(time.nanosecs(), newEntry)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get evaluator +////////////////////////////////////////////////////////////////////////////////////////////// +// TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam::Time& time) const { + +// // Check that map is not empty +// if (knotMap_.empty()) { +// throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); +// } + +// // Get iterator to first element with time equal to or great than 'time' +// std::map::const_iterator it1 +// = knotMap_.lower_bound(time.nanosecs()); + +// // Check if time is passed the last entry +// if (it1 == knotMap_.end()) { + +// // If we allow extrapolation, return constant-velocity interpolated entry +// if (allowExtrapolation_) { +// --it1; // should be safe, as we checked that the map was not empty.. +// const SteamCATrajVar::Ptr& endKnot = it1->second; +// TransformEvaluator::Ptr T_t_k = +// ConstVelTransformEvaluator::MakeShared(endKnot->getVelocity(), time - endKnot->getTime()); +// return compose(T_t_k, endKnot->getPose()); +// } else { +// throw std::runtime_error("Requested trajectory evaluator at an invalid time."); +// } +// } + +// // Check if we requested time exactly +// if (it1->second->getTime() == time) { + +// // return state variable exactly (no interp) +// return it1->second->getPose(); +// } + +// // Check if we requested before first time +// if (it1 == knotMap_.begin()) { + +// // If we allow extrapolation, return constant-velocity interpolated entry +// if (allowExtrapolation_) { +// const SteamCATrajVar::Ptr& startKnot = it1->second; +// TransformEvaluator::Ptr T_t_k = +// ConstVelTransformEvaluator::MakeShared(startKnot->getVelocity(), +// time - startKnot->getTime()); +// return compose(T_t_k, startKnot->getPose()); +// } else { +// throw std::runtime_error("Requested trajectory evaluator at an invalid time."); +// } +// } + +// // Get iterators bounding the time interval +// std::map::const_iterator it2 = it1; --it1; +// if (time <= it1->second->getTime() || time >= it2->second->getTime()) { +// throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " +// "should not trigger... report to a STEAM contributor."); +// } + +// // Create interpolated evaluator +// return SteamTrajPoseInterpEval::MakeShared(time, it1->second, it2->second); +// } + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior +/// should exist on a trajectory, adding a second will overwrite the first. +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajInterface::addPosePrior(const steam::Time& time, + const lgmath::se3::Transformation& pose, + const Eigen::Matrix& cov) { + + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][addPosePrior] map was empty."); + } + + // Try to find knot at same time + std::map::const_iterator it = knotMap_.find(time.nanosecs()); + if (it == knotMap_.end()) { + throw std::runtime_error("[GpTrajectory][addPosePrior] no knot at provided time."); + } + + // Get reference + const SteamCATrajVar::Ptr& knotRef = it->second; + + // Check that the pose is not locked + if(!knotRef->getPose()->isActive()) { + throw std::runtime_error("[GpTrajectory][addPosePrior] tried to add prior to locked pose."); + } + + // Set up loss function, noise model, and error function + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + steam::BaseNoiseModel<6>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<6>(cov)); + steam::TransformErrorEval::Ptr errorfunc(new steam::TransformErrorEval(pose, knotRef->getPose())); + + // Create cost term + posePriorFactor_ = steam::WeightedLeastSqCostTerm<6,6>::Ptr( + new steam::WeightedLeastSqCostTerm<6,6>(errorfunc, sharedNoiseModel, sharedLossFunc)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Add a unary velocity prior factor at a knot time. Note that only a single velocity +/// prior should exist on a trajectory, adding a second will overwrite the first. +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajInterface::addVelocityPrior(const steam::Time& time, + const Eigen::Matrix& velocity, + const Eigen::Matrix& cov) { + + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][addVelocityPrior] map was empty."); + } + + // Try to find knot at same time + std::map::const_iterator it = knotMap_.find(time.nanosecs()); + if (it == knotMap_.end()) { + throw std::runtime_error("[GpTrajectory][addVelocityPrior] no knot at provided time."); + } + + // Get reference + const SteamCATrajVar::Ptr& knotRef = it->second; + + // Check that the pose is not locked + if(knotRef->getVelocity()->isLocked()) { + throw std::runtime_error("[GpTrajectory][addVelocityPrior] tried to add prior to locked pose."); + } + + // Set up loss function, noise model, and error function + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + steam::BaseNoiseModel<6>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<6>(cov)); + steam::VectorSpaceErrorEval<6,6>::Ptr errorfunc(new steam::VectorSpaceErrorEval<6,6>(velocity, knotRef->getVelocity())); + + // Create cost term + velocityPriorFactor_ = steam::WeightedLeastSqCostTerm<6,6>::Ptr( + new steam::WeightedLeastSqCostTerm<6,6>(errorfunc, sharedNoiseModel, sharedLossFunc)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get cost terms associated with the prior for unlocked parts of the trajectory +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajInterface::appendPriorCostTerms( + const ParallelizedCostTermCollection::Ptr& costTerms) const { + + // If empty, return none + if (knotMap_.empty()) { + return; + } + + // Check for pose or velocity priors + if (posePriorFactor_) { + costTerms->add(posePriorFactor_); + } + if (velocityPriorFactor_) { + costTerms->add(velocityPriorFactor_); + } + + // All prior factors will use an L2 loss function + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + + // Initialize first iterator + std::map::const_iterator it1 = knotMap_.begin(); + if (it1 == knotMap_.end()) { + throw std::runtime_error("No knots..."); + } + + // Iterate through all states.. if any are unlocked, supply a prior term + std::map::const_iterator it2 = it1; ++it2; + for (; it2 != knotMap_.end(); ++it1, ++it2) { + + // Get knots + const SteamCATrajVar::ConstPtr& knot1 = it1->second; + const SteamCATrajVar::ConstPtr& knot2 = it2->second; + + // Check if any of the variables are unlocked + if(knot1->getPose()->isActive() || !knot1->getVelocity()->isLocked() || + knot2->getPose()->isActive() || !knot2->getVelocity()->isLocked() ) { + + // Generate 18 x 18 information matrix for GP prior factor + Eigen::Matrix Qi_inv; + double one_over_dt = 1.0/(knot2->getTime() - knot1->getTime()).seconds(); + double one_over_dt2 = one_over_dt*one_over_dt; + double one_over_dt3 = one_over_dt2*one_over_dt; + double one_over_dt4 = one_over_dt3*one_over_dt; + double one_over_dt5 = one_over_dt4*one_over_dt; + + Qi_inv.block<6,6>(0,0) = 720.0 * one_over_dt5 * Qc_inv_; + Qi_inv.block<6,6>(6,6) = 192.0 * one_over_dt3 * Qc_inv_; + Qi_inv.block<6,6>(12,12) = 9.0 * one_over_dt * Qc_inv_; + Qi_inv.block<6,6>(6,0) = Qi_inv.block<6,6>(0,6) = -360.0 * one_over_dt4 * Qc_inv_; + Qi_inv.block<6,6>(12,0) = Qi_inv.block<6,6>(0,12) = 60.0 * one_over_dt3 * Qc_inv_; + Qi_inv.block<6,6>(12,6) = Qi_inv.block<6,6>(6,12) = -36.0 * one_over_dt2 * Qc_inv_; + + steam::BaseNoiseModelX::Ptr sharedGPNoiseModel( + new steam::StaticNoiseModelX(Qi_inv, steam::INFORMATION)); + + // Create cost term + steam::se3::SteamCATrajPriorFactor::Ptr errorfunc( + new steam::se3::SteamCATrajPriorFactor(knot1, knot2)); + steam::WeightedLeastSqCostTermX::Ptr cost( + new steam::WeightedLeastSqCostTermX(errorfunc, sharedGPNoiseModel, sharedLossFunc)); + costTerms->add(cost); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get active state variables in the trajectory +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajInterface::getActiveStateVariables( + std::map* outStates) const { + + // Iterate over trajectory + std::map::const_iterator it; + for (it = knotMap_.begin(); it != knotMap_.end(); ++it) { + + // Append active states in transform evaluator + it->second->getPose()->getActiveStateVariables(outStates); + + // Check if velocity is locked + if (!it->second->getVelocity()->isLocked()) { + (*outStates)[it->second->getVelocity()->getKey().getID()] = it->second->getVelocity(); + } + } +} + +} // se3 +} // steam diff --git a/src/trajectory_ca/SteamCATrajPriorFactor.cpp b/src/trajectory_ca/SteamCATrajPriorFactor.cpp new file mode 100644 index 0000000..69d69e5 --- /dev/null +++ b/src/trajectory_ca/SteamCATrajPriorFactor.cpp @@ -0,0 +1,189 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajPriorFactor.cpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +SteamCATrajPriorFactor::SteamCATrajPriorFactor(const SteamCATrajVar::ConstPtr& knot1, + const SteamCATrajVar::ConstPtr& knot2) : + knot1_(knot1), knot2_(knot2) { +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool SteamCATrajPriorFactor::isActive() const { + return knot1_->getPose()->isActive() || !knot1_->getVelocity()->isLocked() || + knot2_->getPose()->isActive() || !knot2_->getVelocity()->isLocked(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the GP prior factor +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::VectorXd SteamCATrajPriorFactor::evaluate() const { + + // Precompute values + lgmath::se3::Transformation T_21 = knot2_->getPose()->evaluate()/knot1_->getPose()->evaluate(); + Eigen::Matrix xi_21 = T_21.vec(); + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + Eigen::Matrix w = lgmath::se3::curlyhat(J_21_inv*knot2_->getVelocity()->getValue()); + + // Compute error + Eigen::Matrix error; + double dt = (knot2_->getTime() - knot1_->getTime()).seconds(); + double dt2 = dt*dt; + error.head<6>() = xi_21 - dt*knot1_->getVelocity()->getValue() - 0.5*dt2*knot1_->getAcceleration()->getValue(); + error.segment<6>(6) = J_21_inv*knot2_->getVelocity()->getValue() - knot1_->getVelocity()->getValue() - dt*knot1_->getAcceleration()->getValue(); + error.tail<6>() = -0.5*w*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue() - knot1_->getAcceleration()->getValue(); + return error; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the GP prior factor and Jacobians +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::VectorXd SteamCATrajPriorFactor::evaluate(const Eigen::MatrixXd& lhs, + std::vector >* jacs) const { + + // Check and initialize jacobian array + if (jacs == NULL) { + throw std::invalid_argument("Null pointer provided to return-input 'jacs' in evaluate"); + } + jacs->clear(); + + // Get evaluation trees + EvalTreeNode* evaluationTree1 = knot1_->getPose()->evaluateTree(); + EvalTreeNode* evaluationTree2 = knot2_->getPose()->evaluateTree(); + + // Compute intermediate values + lgmath::se3::Transformation T_21 = evaluationTree2->getValue()/evaluationTree1->getValue(); + Eigen::Matrix xi_21 = T_21.vec(); + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + Eigen::Matrix w = lgmath::se3::curlyhat(J_21_inv*knot2_->getVelocity()->getValue()); + Eigen::Matrix v2curl = lgmath::se3::curlyhat(knot2_->getVelocity()->getValue()); + Eigen::Matrix a2curl = lgmath::se3::curlyhat(knot2_->getAcceleration()->getValue()); + + double deltaTime = (knot2_->getTime() - knot1_->getTime()).seconds(); + double dt2 = deltaTime*deltaTime; + + // Knot 1 transform + if(knot1_->getPose()->isActive()) { + Eigen::Matrix Jinv_12 = J_21_inv*T_21.adjoint(); + + // Construct jacobian + Eigen::Matrix jacobian; + jacobian.topRows<6>() = -Jinv_12; + jacobian.block<6,6>(6,0) = -0.5*lgmath::se3::curlyhat(knot2_->getVelocity()->getValue())*Jinv_12; + jacobian.bottomRows<6>() = -0.25*v2curl*v2curl*Jinv_12 - 0.5*a2curl*Jinv_12; + + // Get Jacobians + knot1_->getPose()->appendBlockAutomaticJacobians(lhs * jacobian, evaluationTree1, jacs); + } + + // Get index of split between left and right-hand-side of pose Jacobians + unsigned int hintIndex = jacs->size(); + + // Knot 2 transform + if(knot2_->getPose()->isActive()) { + + // Construct jacobian + Eigen::Matrix jacobian; + jacobian.topRows<6>() = J_21_inv; + jacobian.block<6,6>(6,0) = 0.5*lgmath::se3::curlyhat(knot2_->getVelocity()->getValue())*J_21_inv; + jacobian.bottomRows<6>() = 0.25*v2curl*v2curl*J_21_inv + 0.5*a2curl*J_21_inv; + + // Get Jacobians + knot2_->getPose()->appendBlockAutomaticJacobians(lhs * jacobian, evaluationTree2, jacs); + } + + // Merge jacobians (transform evaluators from knots 1 and 2 could contain the same + // state variables, in which case we need to merge) + Jacobian<>::merge(jacs, hintIndex); + + // Knot 1 velocity + if(!knot1_->getVelocity()->isLocked()) { + + // Construct Jacobian Object + jacs->push_back(Jacobian<>()); + Jacobian<>& jacref = jacs->back(); + jacref.key = knot1_->getVelocity()->getKey(); + + // Fill in matrix + Eigen::Matrix jacobian; + jacobian.topRows<6>() = -deltaTime*Eigen::Matrix::Identity(); + jacobian.block<6,6>(6,0) = -Eigen::Matrix::Identity(); + jacobian.bottomRows<6>() = Eigen::Matrix::Zero(); + jacref.jac = lhs * jacobian; + } + + // Knot 2 velocity + if(!knot2_->getVelocity()->isLocked()) { + + // Construct Jacobian Object + jacs->push_back(Jacobian<>()); + Jacobian<>& jacref = jacs->back(); + jacref.key = knot2_->getVelocity()->getKey(); + + // Fill in matrix + Eigen::Matrix jacobian; + jacobian.topRows<6>() = Eigen::Matrix::Zero(); + jacobian.block<6,6>(6,0) = J_21_inv; + jacobian.bottomRows<6>() = -0.5*w + 0.5*v2curl*J_21_inv; + jacref.jac = lhs * jacobian; + } + + // Knot 1 acceleration + if(!knot1_->getAcceleration()->isLocked()) { + + // Construct Jacobian Object + jacs->push_back(Jacobian<>()); + Jacobian<>& jacref = jacs->back(); + jacref.key = knot1_->getAcceleration()->getKey(); + + // Fill in matrix + Eigen::Matrix jacobian; + jacobian.topRows<6>() = -0.5*dt2*Eigen::Matrix::Identity(); + jacobian.block<6,6>(6,0) = -deltaTime*Eigen::Matrix::Identity(); + jacobian.bottomRows<6>() = -Eigen::Matrix::Identity(); + jacref.jac = lhs * jacobian; + } + + // Knot 2 acceleration + if(!knot2_->getAcceleration()->isLocked()) { + + // Construct Jacobian Object + jacs->push_back(Jacobian<>()); + Jacobian<>& jacref = jacs->back(); + jacref.key = knot2_->getAcceleration()->getKey(); + + // Fill in matrix + Eigen::Matrix jacobian; + jacobian.topRows<6>() = Eigen::Matrix::Zero(); + jacobian.block<6,6>(6,0) = Eigen::Matrix::Zero(); + jacobian.bottomRows<6>() = J_21_inv; + jacref.jac = lhs * jacobian; + } + + // Return tree memory to pool + EvalTreeNode::pool.returnObj(evaluationTree1); + EvalTreeNode::pool.returnObj(evaluationTree2); + + // Return error + Eigen::Matrix error; + error.head<6>() = xi_21 - deltaTime*knot1_->getVelocity()->getValue() - 0.5*dt2*knot1_->getAcceleration()->getValue(); + error.segment<6>(6) = J_21_inv*knot2_->getVelocity()->getValue() - knot1_->getVelocity()->getValue() - deltaTime*knot1_->getAcceleration()->getValue(); + error.tail<6>() = -0.5*w*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue() - knot1_->getAcceleration()->getValue(); + return error; +} + +} // se3 +} // steam diff --git a/src/trajectory_ca/SteamTrajVar.cpp b/src/trajectory_ca/SteamCATrajVar.cpp similarity index 97% rename from src/trajectory_ca/SteamTrajVar.cpp rename to src/trajectory_ca/SteamCATrajVar.cpp index 24d50e6..194eaf5 100644 --- a/src/trajectory_ca/SteamTrajVar.cpp +++ b/src/trajectory_ca/SteamCATrajVar.cpp @@ -4,7 +4,7 @@ /// \author Tim Tang, ASRL ////////////////////////////////////////////////////////////////////////////////////////////// -#include +#include #include From 187e308b5ff7aad2d20918f68bdcf5f5019a1a57 Mon Sep 17 00:00:00 2001 From: TangTim Date: Wed, 18 Jul 2018 18:30:25 -0500 Subject: [PATCH 15/31] Fixed a bug in SampleBAandCATrajPrior, can now run the sample problem --- samples/SimpleBAandCATrajPrior.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/samples/SimpleBAandCATrajPrior.cpp b/samples/SimpleBAandCATrajPrior.cpp index b568292..ffcbf56 100644 --- a/samples/SimpleBAandCATrajPrior.cpp +++ b/samples/SimpleBAandCATrajPrior.cpp @@ -142,7 +142,7 @@ int main(int argc, char** argv) { traj_states_ic[0].pose->setLock(true); // Lock the first velocity as well - traj_states_ic[0].velocity->setLock(true); + // traj_states_ic[0].velocity->setLock(true); // Setup relative landmarks landmarks_ic.resize(dataset.land_ic.size()); @@ -249,7 +249,7 @@ int main(int argc, char** argv) { const TrajStateVar& state = traj_states_ic.at(i); problem.addStateVariable(state.pose); problem.addStateVariable(state.velocity); - // problem.addStateVariable(state.acceleration); + problem.addStateVariable(state.acceleration); } // Add landmark variables From bdbb68f24b2ff9d2462aa4fa94ddabd3551161f9 Mon Sep 17 00:00:00 2001 From: TangTim Date: Thu, 19 Jul 2018 16:49:49 -0500 Subject: [PATCH 16/31] Now compiles with interpolation function --- .../SteamCATrajPoseInterpEval.hpp | 139 ++++++++ samples/CMakeLists.txt | 4 + src/trajectory_ca/SteamCATrajInterface.cpp | 118 +++---- .../SteamCATrajPoseInterpEval.cpp | 323 ++++++++++++++++++ 4 files changed, 525 insertions(+), 59 deletions(-) create mode 100644 include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp create mode 100644 src/trajectory_ca/SteamCATrajPoseInterpEval.cpp diff --git a/include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp b/include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp new file mode 100644 index 0000000..f5cc741 --- /dev/null +++ b/include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp @@ -0,0 +1,139 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajPoseInterpEval.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CA_TRAJECTORY_POSE_INTERP_EVAL_HPP +#define STEAM_CA_TRAJECTORY_POSE_INTERP_EVAL_HPP + +#include + +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Simple transform evaluator for a transformation state variable +////////////////////////////////////////////////////////////////////////////////////////////// +class SteamCATrajPoseInterpEval : public TransformEvaluator +{ + public: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajPoseInterpEval(const Time& time, + const SteamCATrajVar::ConstPtr& knot1, + const SteamCATrajVar::ConstPtr& knot2); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Pseudo constructor - return a shared pointer to a new instance + ////////////////////////////////////////////////////////////////////////////////////////////// + static Ptr MakeShared(const Time& time, + const SteamCATrajVar::ConstPtr& knot1, + const SteamCATrajVar::ConstPtr& knot2); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Adds references (shared pointers) to active state variables to the map output + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual void getActiveStateVariables( + std::map* outStates) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the transformation matrix + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual lgmath::se3::Transformation evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the transformation matrix tree + /// + /// ** Note that the returned pointer belongs to the memory pool EvalTreeNode::pool, + /// and should be given back to the pool, rather than being deleted. + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual EvalTreeNode* evaluateTree() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the Jacobian tree + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual void appendBlockAutomaticJacobians( + const Eigen::MatrixXd& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Implementation for Block Automatic Differentiation + ////////////////////////////////////////////////////////////////////////////////////////////// + template + void appendJacobiansImpl(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief First (earlier) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajVar::ConstPtr knot1_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Second (later) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajVar::ConstPtr knot2_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Interpolation coefficients + ////////////////////////////////////////////////////////////////////////////////////////////// + double omega11_; + double omega12_; + double omega13_; + + double lambda12_; + double lambda13_; + + // double psi11_; + // double psi12_; + // double psi21_; + // double psi22_; + // double lambda11_; + // double lambda12_; + // double lambda21_; + // double lambda22_; +}; + +} // se3 +} // steam + +#endif // STEAM_CA_TRAJECTORY_POSE_INTERP_EVAL_HPP diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 8c82045..65d6d87 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -44,3 +44,7 @@ target_link_libraries(SimpleBAandTrajPrior steam ${DEPEND_LIBS}) # simple example of constant acceleration trajectory interface for prior, combined with bundle adjustment add_executable(SimpleBAandCATrajPrior ${CMAKE_CURRENT_SOURCE_DIR}/SimpleBAandCATrajPrior.cpp) target_link_libraries(SimpleBAandCATrajPrior steam ${DEPEND_LIBS}) + +# simple example of constant acceleration trajectory interface for prior, combined with p2p error terms +add_executable(SimpleP2PandCATrajPrior ${CMAKE_CURRENT_SOURCE_DIR}/SimpleP2PandCATrajPrior.cpp) +target_link_libraries(SimpleP2PandCATrajPrior steam ${DEPEND_LIBS}) \ No newline at end of file diff --git a/src/trajectory_ca/SteamCATrajInterface.cpp b/src/trajectory_ca/SteamCATrajInterface.cpp index 79315ed..754854c 100644 --- a/src/trajectory_ca/SteamCATrajInterface.cpp +++ b/src/trajectory_ca/SteamCATrajInterface.cpp @@ -8,7 +8,7 @@ #include -// #include +#include #include #include @@ -78,64 +78,64 @@ void SteamCATrajInterface::add(const steam::Time& time, ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get evaluator ////////////////////////////////////////////////////////////////////////////////////////////// -// TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam::Time& time) const { - -// // Check that map is not empty -// if (knotMap_.empty()) { -// throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); -// } - -// // Get iterator to first element with time equal to or great than 'time' -// std::map::const_iterator it1 -// = knotMap_.lower_bound(time.nanosecs()); - -// // Check if time is passed the last entry -// if (it1 == knotMap_.end()) { - -// // If we allow extrapolation, return constant-velocity interpolated entry -// if (allowExtrapolation_) { -// --it1; // should be safe, as we checked that the map was not empty.. -// const SteamCATrajVar::Ptr& endKnot = it1->second; -// TransformEvaluator::Ptr T_t_k = -// ConstVelTransformEvaluator::MakeShared(endKnot->getVelocity(), time - endKnot->getTime()); -// return compose(T_t_k, endKnot->getPose()); -// } else { -// throw std::runtime_error("Requested trajectory evaluator at an invalid time."); -// } -// } - -// // Check if we requested time exactly -// if (it1->second->getTime() == time) { - -// // return state variable exactly (no interp) -// return it1->second->getPose(); -// } - -// // Check if we requested before first time -// if (it1 == knotMap_.begin()) { - -// // If we allow extrapolation, return constant-velocity interpolated entry -// if (allowExtrapolation_) { -// const SteamCATrajVar::Ptr& startKnot = it1->second; -// TransformEvaluator::Ptr T_t_k = -// ConstVelTransformEvaluator::MakeShared(startKnot->getVelocity(), -// time - startKnot->getTime()); -// return compose(T_t_k, startKnot->getPose()); -// } else { -// throw std::runtime_error("Requested trajectory evaluator at an invalid time."); -// } -// } - -// // Get iterators bounding the time interval -// std::map::const_iterator it2 = it1; --it1; -// if (time <= it1->second->getTime() || time >= it2->second->getTime()) { -// throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " -// "should not trigger... report to a STEAM contributor."); -// } - -// // Create interpolated evaluator -// return SteamTrajPoseInterpEval::MakeShared(time, it1->second, it2->second); -// } +TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam::Time& time) const { + + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); + } + + // Get iterator to first element with time equal to or great than 'time' + std::map::const_iterator it1 + = knotMap_.lower_bound(time.nanosecs()); + + // Check if time is passed the last entry + if (it1 == knotMap_.end()) { + + // If we allow extrapolation, return constant-velocity interpolated entry + if (allowExtrapolation_) { + --it1; // should be safe, as we checked that the map was not empty.. + const SteamCATrajVar::Ptr& endKnot = it1->second; + TransformEvaluator::Ptr T_t_k = + ConstVelTransformEvaluator::MakeShared(endKnot->getVelocity(), time - endKnot->getTime()); + return compose(T_t_k, endKnot->getPose()); + } else { + throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + } + } + + // Check if we requested time exactly + if (it1->second->getTime() == time) { + + // return state variable exactly (no interp) + return it1->second->getPose(); + } + + // Check if we requested before first time + if (it1 == knotMap_.begin()) { + + // If we allow extrapolation, return constant-velocity interpolated entry + if (allowExtrapolation_) { + const SteamCATrajVar::Ptr& startKnot = it1->second; + TransformEvaluator::Ptr T_t_k = + ConstVelTransformEvaluator::MakeShared(startKnot->getVelocity(), + time - startKnot->getTime()); + return compose(T_t_k, startKnot->getPose()); + } else { + throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + } + } + + // Get iterators bounding the time interval + std::map::const_iterator it2 = it1; --it1; + if (time <= it1->second->getTime() || time >= it2->second->getTime()) { + throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " + "should not trigger... report to a STEAM contributor."); + } + + // Create interpolated evaluator + return SteamCATrajPoseInterpEval::MakeShared(time, it1->second, it2->second); +} ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior diff --git a/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp b/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp new file mode 100644 index 0000000..9d01fdd --- /dev/null +++ b/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp @@ -0,0 +1,323 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajPoseInterpEval.cpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +SteamCATrajPoseInterpEval::SteamCATrajPoseInterpEval(const Time& time, + const SteamCATrajVar::ConstPtr& knot1, + const SteamCATrajVar::ConstPtr& knot2) : + knot1_(knot1), knot2_(knot2) { + + // Calculate time constants + double t1 = knot1->getTime().seconds(); + double t2 = knot2->getTime().seconds(); + double tau = time.seconds(); + + double T = (knot2->getTime() - knot1->getTime()).seconds(); + double delta_tau = (time - knot1->getTime()).seconds(); + double delta_kappa = (knot2->getTime()-time).seconds(); + + double T2 = T*T; + double T3 = T2*T; + double T4 = T3*T; + double T5 = T4*T; + + double delta_tau2 = delta_tau*delta_tau; + double delta_tau3 = delta_tau2*delta_tau; + double delta_kappa2 = delta_kappa*delta_kappa; + double delta_kappa3 = delta_kappa2*delta_kappa; + + // Calculate 'omega' interpolation values + omega11_ = delta_tau3/T5*(t1*t1 - 5*t1*t2 + 3*t1*tau + 10*t2*t2 - 15*t2*tau + 6*tau*tau); + omega12_ = delta_tau3*delta_kappa/T4*(t1 - 4*t2 + 3*tau); + omega13_ = delta_tau3*delta_kappa2/(2*T3); + + // Calculate 'lambda' interpolation values + lambda12_ = delta_tau*delta_kappa3/T4*(t2 - 4*t1 + 3*tau); + lambda13_ = delta_tau2*delta_kappa3/(2*T3); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Pseudo constructor - return a shared pointer to a new instance +////////////////////////////////////////////////////////////////////////////////////////////// +SteamCATrajPoseInterpEval::Ptr SteamCATrajPoseInterpEval::MakeShared(const Time& time, + const SteamCATrajVar::ConstPtr& knot1, + const SteamCATrajVar::ConstPtr& knot2) { + return SteamCATrajPoseInterpEval::Ptr(new SteamCATrajPoseInterpEval(time, knot1, knot2)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool SteamCATrajPoseInterpEval::isActive() const { + return knot1_->getPose()->isActive() || + !knot1_->getVelocity()->isLocked() || + knot2_->getPose()->isActive() || + !knot2_->getVelocity()->isLocked(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Adds references (shared pointers) to active state variables to the map output +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::getActiveStateVariables( + std::map* outStates) const { + + knot1_->getPose()->getActiveStateVariables(outStates); + knot2_->getPose()->getActiveStateVariables(outStates); + if (!knot1_->getVelocity()->isLocked()) { + (*outStates)[knot1_->getVelocity()->getKey().getID()] = knot1_->getVelocity(); + } + if (!knot2_->getVelocity()->isLocked()) { + (*outStates)[knot2_->getVelocity()->getKey().getID()] = knot2_->getVelocity(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the transformation matrix +////////////////////////////////////////////////////////////////////////////////////////////// +lgmath::se3::Transformation SteamCATrajPoseInterpEval::evaluate() const { + + // Get relative matrix info + lgmath::se3::Transformation T_21 = knot2_->getPose()->evaluate()/knot1_->getPose()->evaluate(); + + // Get se3 algebra of relative matrix + Eigen::Matrix xi_21 = T_21.vec(); + + // Calculate the 6x6 associated Jacobian + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + + // Intermediate variable + Eigen::Matrix varpicurl2 = lgmath::se3::curlyhat(J_21_inv*knot2_->getVelocity()->getValue()); + + // Calculate interpolated relative se3 algebra + Eigen::Matrix xi_i1 = lambda12_*knot1_->getVelocity()->getValue() + + lambda13_*knot1_->getAcceleration()->getValue() + + omega11_*xi_21 + + omega12_*J_21_inv*knot2_->getVelocity()->getValue()+ + omega13_*(-0.5*varpicurl2*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue()); + + // Calculate interpolated relative transformation matrix + lgmath::se3::Transformation T_i1(xi_i1); + + // Return `global' interpolated transform + return T_i1*knot1_->getPose()->evaluate(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the transformation matrix tree +////////////////////////////////////////////////////////////////////////////////////////////// +EvalTreeNode* SteamCATrajPoseInterpEval::evaluateTree() const { + + // Evaluate sub-trees + EvalTreeNode* transform1 = knot1_->getPose()->evaluateTree(); + EvalTreeNode* transform2 = knot2_->getPose()->evaluateTree(); + + // Get relative matrix info + lgmath::se3::Transformation T_21 = transform2->getValue()/transform1->getValue(); + + // Get se3 algebra of relative matrix + Eigen::Matrix xi_21 = T_21.vec(); + + // Calculate the 6x6 associated Jacobian + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + + // Intermediate variable + Eigen::Matrix varpicurl2 = lgmath::se3::curlyhat(J_21_inv*knot2_->getVelocity()->getValue()); + + // Calculate interpolated relative se3 algebra + Eigen::Matrix xi_i1 = lambda12_*knot1_->getVelocity()->getValue() + + lambda13_*knot1_->getAcceleration()->getValue() + + omega11_*xi_21 + + omega12_*J_21_inv*knot2_->getVelocity()->getValue()+ + omega13_*(-0.5*varpicurl2*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue()); + + // Calculate interpolated relative transformation matrix + lgmath::se3::Transformation T_i1(xi_i1); + + // Interpolated relative transform - new root node (using pool memory) + EvalTreeNode* root = EvalTreeNode::pool.getObj(); + root->setValue(T_i1*transform1->getValue()); + + // Add children + root->addChild(transform1); + root->addChild(transform2); + + // Return new root node + return root; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Implementation for Block Automatic Differentiation +////////////////////////////////////////////////////////////////////////////////////////////// +template +void SteamCATrajPoseInterpEval::appendJacobiansImpl( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + + // Cast back to transformations + EvalTreeNode* transform1 = + static_cast*>(evaluationTree->childAt(0)); + EvalTreeNode* transform2 = + static_cast*>(evaluationTree->childAt(1)); + + // Get relative matrix info + lgmath::se3::Transformation T_21 = transform2->getValue()/transform1->getValue(); + + // Get se3 algebra of relative matrix + Eigen::Matrix xi_21 = T_21.vec(); + + // Calculate the 6x6 associated Jacobian + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + + // Intermediate variable + Eigen::Matrix varpicurl2 = lgmath::se3::curlyhat(J_21_inv*knot2_->getVelocity()->getValue()); + Eigen::Matrix varpicurl = lgmath::se3::curlyhat(knot2_->getVelocity()->getValue()); + + // Calculate interpolated relative se3 algebra + Eigen::Matrix xi_i1 = lambda12_*knot1_->getVelocity()->getValue() + + lambda13_*knot1_->getAcceleration()->getValue() + + omega11_*xi_21 + + omega12_*J_21_inv*knot2_->getVelocity()->getValue()+ + omega13_*(-0.5*varpicurl2*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue()); + + // Calculate interpolated relative transformation matrix + lgmath::se3::Transformation T_i1(xi_i1); + + // Calculate the 6x6 Jacobian associated with the interpolated relative transformation matrix + Eigen::Matrix J_i1 = lgmath::se3::vec2jac(xi_i1); + + // Check if evaluator is active + if (this->isActive()) { + + // Pose Jacobians + if (knot1_->getPose()->isActive() || knot2_->getPose()->isActive()) { + + // Precompute matrix + Eigen::Matrix w = omega11_*J_i1*J_21_inv + + 0.5*omega12_*J_i1*varpicurl*J_21_inv + 0.25*omega13_*J_i1*varpicurl*varpicurl*J_21_inv+ + 0.5*omega13_*J_i1*lgmath::se3::curlyhat(knot2_->getAcceleration()->getValue())*J_21_inv; + + // Check if transform1 is active + if (knot1_->getPose()->isActive()) { + Eigen::Matrix jacobian = (-1) * w * T_21.adjoint() + T_i1.adjoint(); + knot1_->getPose()->appendBlockAutomaticJacobians(lhs*jacobian, transform1, outJacobians); + } + + // Get index of split between left and right-hand-side of Jacobians + unsigned int hintIndex = outJacobians->size(); + + // Check if transform2 is active + if (knot2_->getPose()->isActive()) { + knot2_->getPose()->appendBlockAutomaticJacobians(lhs*w, transform2, outJacobians); + } + + // Merge jacobians + Jacobian::merge(outJacobians, hintIndex); + } + + // 6 x 6 Velocity Jacobian 1 + if(!knot1_->getVelocity()->isLocked()) { + + // Add Jacobian + outJacobians->push_back(Jacobian(knot1_->getVelocity()->getKey(), lhs*lambda12_*J_i1)); + } + + // 6 x 6 Velocity Jacobian 2 + if(!knot2_->getVelocity()->isLocked()) { + + // Add Jacobian + Eigen::Matrix jacobian = omega12_*J_i1*J_21_inv - 0.5*omega13_*J_i1*(varpicurl2 - varpicurl*J_21_inv); + outJacobians->push_back(Jacobian(knot2_->getVelocity()->getKey(), lhs*jacobian)); + } + + // 6 x 6 Acceleration Jacobian 1 + if(!knot1_->getAcceleration()->isLocked()) { + + // Add Jacobian + outJacobians->push_back(Jacobian(knot1_->getAcceleration()->getKey(), lhs*lambda13_*J_i1)); + } + + // 6 x 6 Acceleration Jacobian 2 + if(!knot2_->getAcceleration()->isLocked()) { + + // Add Jacobian + Eigen::Matrix jacobian = omega13_*J_i1*J_21_inv; + outJacobians->push_back(Jacobian(knot2_->getAcceleration()->getKey(), lhs*jacobian)); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::MatrixXd& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Fixed-size evaluations of the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Fixed-size evaluations of the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Fixed-size evaluations of the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Fixed-size evaluations of the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Fixed-size evaluations of the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +} // se3 +} // steam From 51b97cfd817c7765a9367d62700894967658761c Mon Sep 17 00:00:00 2001 From: TangTim Date: Thu, 19 Jul 2018 18:21:32 -0500 Subject: [PATCH 17/31] Interpolation functions have passed sample tests --- .../data/PointToPoint/points_simulated.txt | 400 ++++++++++++++++++ include/steam/data/PointToPoint/poses_ic.txt | 40 ++ .../points_simulated.txt | 400 ++++++++++++++++++ .../data/PointToPoint_distorted/poses_ic.txt | 40 ++ include/steam/data/points_simulated.txt | 400 ++++++++++++++++++ samples/CMakeLists.txt | 6 +- samples/MotionDistortedP2PandCATrajPrior.cpp | 236 +++++++++++ samples/SimpleP2PandCATrajPrior.cpp | 235 ++++++++++ 8 files changed, 1756 insertions(+), 1 deletion(-) create mode 100644 include/steam/data/PointToPoint/points_simulated.txt create mode 100644 include/steam/data/PointToPoint/poses_ic.txt create mode 100644 include/steam/data/PointToPoint_distorted/points_simulated.txt create mode 100644 include/steam/data/PointToPoint_distorted/poses_ic.txt create mode 100644 include/steam/data/points_simulated.txt create mode 100644 samples/MotionDistortedP2PandCATrajPrior.cpp create mode 100644 samples/SimpleP2PandCATrajPrior.cpp diff --git a/include/steam/data/PointToPoint/points_simulated.txt b/include/steam/data/PointToPoint/points_simulated.txt new file mode 100644 index 0000000..12bf6e0 --- /dev/null +++ b/include/steam/data/PointToPoint/points_simulated.txt @@ -0,0 +1,400 @@ +-26.151,-4.4462,34.933,1 +-16.45,-74.15,-52.728,1 +30.849,69.365,34.645,1 +-0.16151,4.2019,131.2,1 +-113.16,-22.276,35.86,1 +4.4604,46.248,-45.118,1 +-11.489,6.6153,-2.7868,1 +74.286,140.76,-0.69856,1 +6.8027,-61.818,90.153,1 +32.477,129.5,81.223,1 +23.259,-9.6475,-114.41,1 +-32.788,24.984,-14.248,1 +-20.835,82.308,-59.545,1 +-53.601,0.42286,9.7717,1 +26.161,74.836,-2.1497,1 +-37.459,-72.008,73.002,1 +38.287,-29.968,-33.513,1 +-10.775,-5.9006,14.947,1 +38.304,-19.489,-23.726,1 +83.951,37.293,-49.289,1 +-26.201,-4.4463,34.933,1 +-16.55,-74.15,-52.728,1 +30.698,69.366,34.645,1 +-0.3614,4.2019,131.2,1 +-113.41,-22.279,35.86,1 +4.1588,46.248,-45.118,1 +-11.838,6.6146,-2.7868,1 +73.876,140.77,-0.69856,1 +6.36,-61.817,90.153,1 +31.962,129.5,81.223,1 +22.712,-9.6439,-114.41,1 +-33.39,24.978,-14.248,1 +-21.5,82.304,-59.545,1 +-54.298,0.40968,9.7717,1 +25.393,74.844,-2.1497,1 +-38.233,-72.02,73.002,1 +37.451,-29.954,-33.513,1 +-11.668,-5.9051,14.947,1 +37.368,-19.472,-23.726,1 +82.938,37.335,-49.289,1 +-27.193,-4.4609,34.933,1 +-17.499,-74.161,-52.728,1 +29.66,69.385,34.645,1 +-1.3573,4.2013,131.2,1 +-114.38,-22.365,35.86,1 +3.1298,46.251,-45.118,1 +-12.836,6.6042,-2.7868,1 +72.758,140.83,-0.69856,1 +5.4282,-61.812,90.153,1 +30.843,129.53,81.223,1 +21.732,-9.6203,-114.41,1 +-34.407,24.941,-14.248,1 +-22.583,82.279,-59.545,1 +-55.287,0.34435,9.7717,1 +24.312,74.875,-2.1497,1 +-39.126,-72.07,73.002,1 +36.505,-29.904,-33.513,1 +-12.646,-5.9217,14.947,1 +36.41,-19.418,-23.726,1 +81.896,37.459,-49.289,1 +-28.171,-4.5034,34.933,1 +-18.365,-74.189,-52.728,1 +28.562,69.434,34.645,1 +-2.3475,4.1987,131.2,1 +-115.33,-22.565,35.86,1 +2.0646,46.257,-45.118,1 +-13.829,6.5803,-2.7868,1 +71.509,140.97,-0.69856,1 +4.5683,-61.801,90.153,1 +29.604,129.59,81.223,1 +20.772,-9.5757,-114.41,1 +-35.438,24.869,-14.248,1 +-23.739,82.23,-59.545,1 +-56.265,0.22298,9.7717,1 +23.166,74.93,-2.1497,1 +-39.937,-72.159,73.002,1 +35.599,-29.818,-33.513,1 +-13.608,-5.9517,14.947,1 +35.482,-19.328,-23.726,1 +80.828,37.664,-49.289,1 +-29.134,-4.5749,34.933,1 +-19.146,-74.236,-52.728,1 +27.404,69.51,34.645,1 +-3.3318,4.1926,131.2,1 +-116.24,-22.882,35.86,1 +0.96303,46.263,-45.118,1 +-14.819,6.5413,-2.7868,1 +70.129,141.18,-0.69856,1 +3.78,-61.787,90.153,1 +28.245,129.68,81.223,1 +19.832,-9.5113,-114.41,1 +-36.484,24.76,-14.248,1 +-24.966,82.156,-59.545,1 +-57.234,0.044133,9.7717,1 +21.955,75.006,-2.1497,1 +-40.666,-72.289,73.002,1 +34.732,-29.697,-33.513,1 +-14.554,-5.9966,14.947,1 +34.583,-19.204,-23.726,1 +79.73,37.948,-49.289,1 +-30.082,-4.677,34.933,1 +-19.842,-74.303,-52.728,1 +26.187,69.611,34.645,1 +-4.3103,4.1816,131.2,1 +-117.11,-23.316,35.86,1 +-0.17479,46.267,-45.118,1 +-15.806,6.4858,-2.7868,1 +68.617,141.45,-0.69856,1 +3.0636,-61.769,90.153,1 +26.766,129.8,81.223,1 +18.911,-9.4287,-114.41,1 +-37.544,24.613,-14.248,1 +-26.265,82.055,-59.545,1 +-58.191,-0.19359,9.7717,1 +20.678,75.101,-2.1497,1 +-41.312,-72.461,73.002,1 +33.904,-29.543,-33.513,1 +-15.483,-6.0577,14.947,1 +33.713,-19.047,-23.726,1 +78.604,38.309,-49.289,1 +-31.015,-4.811,34.933,1 +-20.454,-74.391,-52.728,1 +24.909,69.734,34.645,1 +-5.2829,4.1642,131.2,1 +-117.95,-23.869,35.86,1 +-1.3489,46.269,-45.118,1 +-16.789,6.4124,-2.7868,1 +66.972,141.79,-0.69856,1 +2.4187,-61.75,90.153,1 +25.166,129.93,81.223,1 +18.009,-9.329,-114.41,1 +-38.618,24.425,-14.248,1 +-27.636,81.923,-59.545,1 +-59.138,-0.49155,9.7717,1 +19.336,75.214,-2.1497,1 +-41.874,-72.674,73.002,1 +33.115,-29.356,-33.513,1 +-16.396,-6.1363,14.947,1 +32.871,-18.859,-23.726,1 +77.447,38.746,-49.289,1 +-31.932,-4.9782,34.933,1 +-20.981,-74.499,-52.728,1 +23.571,69.879,34.645,1 +-6.2497,4.139,131.2,1 +-118.76,-24.542,35.86,1 +-2.5593,46.266,-45.118,1 +-17.768,6.3195,-2.7868,1 +65.193,142.19,-0.69856,1 +1.8455,-61.728,90.153,1 +23.446,130.09,81.223,1 +17.125,-9.2134,-114.41,1 +-39.706,24.196,-14.248,1 +-29.078,81.758,-59.545,1 +-60.072,-0.85109,9.7717,1 +17.928,75.34,-2.1497,1 +-42.353,-72.929,73.002,1 +32.364,-29.138,-33.513,1 +-17.293,-6.2336,14.947,1 +32.056,-18.639,-23.726,1 +76.258,39.256,-49.289,1 +-32.833,-5.1798,34.933,1 +-21.423,-74.629,-52.728,1 +22.173,70.042,34.645,1 +-7.2104,4.1046,131.2,1 +-119.52,-25.335,35.86,1 +-3.806,46.255,-45.118,1 +-18.742,6.2058,-2.7868,1 +63.279,142.64,-0.69856,1 +1.3439,-61.705,90.153,1 +21.605,130.26,81.223,1 +16.26,-9.0832,-114.41,1 +-40.806,23.923,-14.248,1 +-30.591,81.558,-59.545,1 +-60.992,-1.2735,9.7717,1 +16.454,75.479,-2.1497,1 +-42.746,-73.226,73.002,1 +31.651,-28.889,-33.513,1 +-18.172,-6.3508,14.947,1 +31.268,-18.389,-23.726,1 +75.036,39.837,-49.289,1 +-33.718,-5.417,34.933,1 +-21.78,-74.779,-52.728,1 +20.712,70.22,34.645,1 +-8.165,4.0597,131.2,1 +-120.24,-26.25,35.86,1 +-5.0888,46.235,-45.118,1 +-19.712,6.0698,-2.7868,1 +61.229,143.15,-0.69856,1 +0.91367,-61.681,90.153,1 +19.642,130.44,81.223,1 +15.413,-8.9393,-114.41,1 +-41.917,23.605,-14.248,1 +-32.173,81.319,-59.545,1 +-61.898,-1.76,9.7717,1 +14.913,75.626,-2.1497,1 +-43.054,-73.564,73.002,1 +30.974,-28.609,-33.513,1 +-19.035,-6.4891,14.947,1 +30.507,-18.11,-23.726,1 +73.78,40.488,-49.289,1 +-34.585,-5.691,34.933,1 +-22.05,-74.949,-52.728,1 +19.19,70.41,34.645,1 +-9.1133,4.0028,131.2,1 +-120.92,-27.287,35.86,1 +-6.4078,46.203,-45.118,1 +-20.677,5.9101,-2.7868,1 +59.042,143.71,-0.69856,1 +0.55482,-61.654,90.153,1 +17.557,130.63,81.223,1 +14.583,-8.7829,-114.41,1 +-43.04,23.239,-14.248,1 +-33.825,81.039,-59.545,1 +-62.787,-2.3118,9.7717,1 +13.305,75.778,-2.1497,1 +-43.276,-73.943,73.002,1 +30.334,-28.3,-33.513,1 +-19.879,-6.6496,14.947,1 +29.771,-17.802,-23.726,1 +72.486,41.205,-49.289,1 +-35.434,-6.0028,34.933,1 +-22.234,-75.139,-52.728,1 +17.606,70.611,34.645,1 +-10.055,3.9326,131.2,1 +-121.55,-28.446,35.86,1 +-7.7628,46.157,-45.118,1 +-21.636,5.7254,-2.7868,1 +56.715,144.3,-0.69856,1 +0.26723,-61.626,90.153,1 +15.349,130.82,81.223,1 +13.771,-8.615,-114.41,1 +-44.172,22.825,-14.248,1 +-35.544,80.713,-59.545,1 +-63.659,-2.93,9.7717,1 +11.629,75.933,-2.1497,1 +-43.409,-74.362,73.002,1 +29.728,-27.962,-33.513,1 +-20.706,-6.8332,14.947,1 +29.059,-17.468,-23.726,1 +71.154,41.986,-49.289,1 +-36.264,-6.3536,34.933,1 +-22.331,-75.348,-52.728,1 +15.959,70.817,34.645,1 +-10.99,3.8478,131.2,1 +-122.13,-29.728,35.86,1 +-9.1536,46.094,-45.118,1 +-22.589,5.5143,-2.7868,1 +54.247,144.94,-0.69856,1 +0.050762,-61.594,90.153,1 +13.019,131,81.223,1 +12.975,-8.4365,-114.41,1 +-45.312,22.359,-14.248,1 +-37.331,80.338,-59.545,1 +-64.511,-3.6157,9.7717,1 +9.8862,76.086,-2.1497,1 +-43.454,-74.82,73.002,1 +29.156,-27.595,-33.513,1 +-21.513,-7.0409,14.947,1 +28.371,-17.106,-23.726,1 +69.781,42.83,-49.289,1 +-37.074,-6.7441,34.933,1 +-22.341,-75.574,-52.728,1 +14.248,71.026,34.645,1 +-11.919,3.7471,131.2,1 +-122.66,-31.132,35.86,1 +-10.58,46.012,-45.118,1 +-23.535,5.2754,-2.7868,1 +51.636,145.6,-0.69856,1 +-0.094747,-61.559,90.153,1 +10.565,131.18,81.223,1 +12.195,-8.2483,-114.41,1 +-46.459,21.841,-14.248,1 +-39.184,79.911,-59.545,1 +-65.341,-4.3698,9.7717,1 +8.0748,76.235,-2.1497,1 +-43.409,-75.316,73.002,1 +28.617,-27.2,-33.513,1 +-22.301,-7.2736,14.947,1 +27.705,-16.718,-23.726,1 +68.364,43.732,-49.289,1 +-37.862,-7.1753,34.933,1 +-22.262,-75.816,-52.728,1 +12.474,71.234,34.645,1 +-12.84,3.6293,131.2,1 +-123.13,-32.659,35.86,1 +-12.041,45.907,-45.118,1 +-24.474,5.0074,-2.7868,1 +48.881,146.29,-0.69856,1 +-0.16949,-61.519,90.153,1 +7.9871,131.34,81.223,1 +11.431,-8.0513,-114.41,1 +-47.612,21.269,-14.248,1 +-41.102,79.427,-59.545,1 +-66.147,-5.1932,9.7717,1 +6.195,76.374,-2.1497,1 +-43.272,-75.847,73.002,1 +28.109,-26.776,-33.513,1 +-23.07,-7.5322,14.947,1 +27.06,-16.304,-23.726,1 +66.9,44.691,-49.289,1 +-38.627,-7.6481,34.933,1 +-22.094,-76.072,-52.728,1 +10.635,71.438,34.645,1 +-13.753,3.493,131.2,1 +-123.53,-34.309,35.86,1 +-13.537,45.777,-45.118,1 +-25.404,4.7091,-2.7868,1 +45.98,147,-0.69856,1 +-0.17369,-61.473,90.153,1 +5.2856,131.49,81.223,1 +10.681,-7.8463,-114.41,1 +-48.768,20.639,-14.248,1 +-43.082,78.882,-59.545,1 +-66.927,-6.0867,9.7717,1 +4.2465,76.5,-2.1497,1 +-43.042,-76.412,73.002,1 +27.632,-26.323,-33.513,1 +-23.817,-7.8174,14.947,1 +26.435,-15.865,-23.726,1 +65.388,45.702,-49.289,1 +-39.369,-8.163,34.933,1 +-21.838,-76.34,-52.728,1 +8.7318,71.632,34.645,1 +-14.658,3.337,131.2,1 +-123.87,-36.08,35.86,1 +-15.068,45.619,-45.118,1 +-26.325,4.3792,-2.7868,1 +42.931,147.72,-0.69856,1 +-0.10764,-61.42,90.153,1 +2.4604,131.6,81.223,1 +9.9461,-7.6342,-114.41,1 +-49.926,19.952,-14.248,1 +-45.123,78.272,-59.545,1 +-67.677,-7.051,9.7717,1 +2.2293,76.609,-2.1497,1 +-42.717,-77.009,73.002,1 +27.184,-25.843,-33.513,1 +-24.542,-8.1299,14.947,1 +25.829,-15.401,-23.726,1 +63.824,46.764,-49.289,1 +-40.084,-8.7209,34.933,1 +-21.491,-76.618,-52.728,1 +6.7633,71.814,34.645,1 +-15.555,3.1602,131.2,1 +-124.13,-37.973,35.86,1 +-16.632,45.43,-45.118,1 +-27.236,4.0165,-2.7868,1 +39.733,148.43,-0.69856,1 +0.028312,-61.357,90.153,1 +-0.48864,131.68,81.223,1 +9.2246,-7.4157,-114.41,1 +-51.083,19.204,-14.248,1 +-47.223,77.593,-59.545,1 +-68.395,-8.0866,9.7717,1 +0.14335,76.695,-2.1497,1 +-42.297,-77.634,73.002,1 +26.762,-25.334,-33.513,1 +-25.245,-8.4703,14.947,1 +25.24,-14.913,-23.726,1 +62.205,47.871,-49.289,1 +-40.773,-9.3221,34.933,1 +-21.054,-76.903,-52.728,1 +4.7294,71.977,34.645,1 +-16.442,2.9614,131.2,1 +-124.31,-39.985,35.86,1 +-18.229,45.206,-45.118,1 +-28.135,3.6199,-2.7868,1 +36.384,149.15,-0.69856,1 +0.23376,-61.284,90.153,1 +-3.561,131.72,81.223,1 +8.5162,-7.1915,-114.41,1 +-52.237,18.394,-14.248,1 +-49.379,76.839,-59.545,1 +-69.078,-9.1939,9.7717,1 +-2.0113,76.754,-2.1497,1 +-41.779,-78.286,73.002,1 +26.367,-24.797,-33.513,1 +-25.924,-8.8392,14.947,1 +24.666,-14.402,-23.726,1 +60.528,49.022,-49.289,1 +-41.432,-9.9671,34.933,1 +-20.526,-77.193,-52.728,1 +2.6301,72.119,34.645,1 +-17.32,2.7395,131.2,1 +-124.41,-42.115,35.86,1 +-19.857,44.945,-45.118,1 +-29.022,3.1882,-2.7868,1 +32.883,149.85,-0.69856,1 +0.50821,-61.197,90.153,1 +-6.7563,131.7,81.223,1 +7.82,-6.9624,-114.41,1 +-53.385,17.521,-14.248,1 +-51.589,76.006,-59.545,1 +-69.723,-10.373,9.7717,1 +-4.2344,76.782,-2.1497,1 +-41.162,-78.96,73.002,1 +25.996,-24.232,-33.513,1 +-26.579,-9.237,14.947,1 +24.106,-13.866,-23.726,1 +58.791,50.211,-49.289,1 diff --git a/include/steam/data/PointToPoint/poses_ic.txt b/include/steam/data/PointToPoint/poses_ic.txt new file mode 100644 index 0000000..7c796b8 --- /dev/null +++ b/include/steam/data/PointToPoint/poses_ic.txt @@ -0,0 +1,40 @@ +1,0,0,0,0,1,0,0,0,0,1,0 +1,-0.0005,0,-0.995,0.0005,1,0,-0.00024875,0,0,1,0 +1,-0.002,0,-1.98,0.002,1,0,-0.00198,0,0,1,0 +0.99999,-0.0045,0,-2.955,0.0045,0.99999,0,-0.0066487,0,0,1,0 +0.99997,-0.0079999,0,-3.92,0.0079999,0.99997,0,-0.01568,0,0,1,0 +0.99992,-0.0125,0,-4.8749,0.0125,0.99992,0,-0.030468,0,0,1,0 +0.99984,-0.017999,0,-5.8197,0.017999,0.99984,0,-0.052379,0,0,1,0 +0.9997,-0.024498,0,-6.7543,0.024498,0.9997,0,-0.082745,0,0,1,0 +0.99949,-0.031995,0,-7.6787,0.031995,0.99949,0,-0.12287,0,0,1,0 +0.99918,-0.040489,0,-8.5927,0.040489,0.99918,0,-0.17402,0,0,1,0 +0.99875,-0.049979,0,-9.496,0.049979,0.99875,0,-0.23745,0,0,1,0 +0.99817,-0.060463,0,-10.389,0.060463,0.99817,0,-0.31435,0,0,1,0 +0.99741,-0.071938,0,-11.27,0.071938,0.99741,0,-0.4059,0,0,1,0 +0.99643,-0.084399,0,-12.141,0.084399,0.99643,0,-0.51324,0,0,1,0 +0.9952,-0.097843,0,-12.999,0.097843,0.9952,0,-0.63747,0,0,1,0 +0.99368,-0.11226,0,-13.846,0.11226,0.99368,0,-0.77965,0,0,1,0 +0.99182,-0.12765,0,-14.68,0.12765,0.99182,0,-0.94079,0,0,1,0 +0.98958,-0.144,0,-15.501,0.144,0.98958,0,-1.1219,0,0,1,0 +0.98691,-0.16129,0,-16.308,0.16129,0.98691,0,-1.3239,0,0,1,0 +0.98375,-0.17952,0,-17.102,0.17952,0.98375,0,-1.5476,0,0,1,0 +-10,0,0,0,0,0,1,0,0,0,0,0.1 +-9.9,0,0,0,0,0.01,1,0,0,0,0,0.1 +-9.8,0,0,0,0,0.02,1,0,0,0,0,0.1 +-9.7,0,0,0,0,0.03,1,0,0,0,0,0.1 +-9.6,0,0,0,0,0.04,1,0,0,0,0,0.1 +-9.5,0,0,0,0,0.05,1,0,0,0,0,0.1 +-9.4,0,0,0,0,0.06,1,0,0,0,0,0.1 +-9.3,0,0,0,0,0.07,1,0,0,0,0,0.1 +-9.2,0,0,0,0,0.08,1,0,0,0,0,0.1 +-9.1,0,0,0,0,0.09,1,0,0,0,0,0.1 +-9,0,0,0,0,0.1,1,0,0,0,0,0.1 +-8.9,0,0,0,0,0.11,1,0,0,0,0,0.1 +-8.8,0,0,0,0,0.12,1,0,0,0,0,0.1 +-8.7,0,0,0,0,0.13,1,0,0,0,0,0.1 +-8.6,0,0,0,0,0.14,1,0,0,0,0,0.1 +-8.5,0,0,0,0,0.15,1,0,0,0,0,0.1 +-8.4,0,0,0,0,0.16,1,0,0,0,0,0.1 +-8.3,0,0,0,0,0.17,1,0,0,0,0,0.1 +-8.2,0,0,0,0,0.18,1,0,0,0,0,0.1 +-8.1,0,0,0,0,0.19,1,0,0,0,0,0.1 diff --git a/include/steam/data/PointToPoint_distorted/points_simulated.txt b/include/steam/data/PointToPoint_distorted/points_simulated.txt new file mode 100644 index 0000000..12bf6e0 --- /dev/null +++ b/include/steam/data/PointToPoint_distorted/points_simulated.txt @@ -0,0 +1,400 @@ +-26.151,-4.4462,34.933,1 +-16.45,-74.15,-52.728,1 +30.849,69.365,34.645,1 +-0.16151,4.2019,131.2,1 +-113.16,-22.276,35.86,1 +4.4604,46.248,-45.118,1 +-11.489,6.6153,-2.7868,1 +74.286,140.76,-0.69856,1 +6.8027,-61.818,90.153,1 +32.477,129.5,81.223,1 +23.259,-9.6475,-114.41,1 +-32.788,24.984,-14.248,1 +-20.835,82.308,-59.545,1 +-53.601,0.42286,9.7717,1 +26.161,74.836,-2.1497,1 +-37.459,-72.008,73.002,1 +38.287,-29.968,-33.513,1 +-10.775,-5.9006,14.947,1 +38.304,-19.489,-23.726,1 +83.951,37.293,-49.289,1 +-26.201,-4.4463,34.933,1 +-16.55,-74.15,-52.728,1 +30.698,69.366,34.645,1 +-0.3614,4.2019,131.2,1 +-113.41,-22.279,35.86,1 +4.1588,46.248,-45.118,1 +-11.838,6.6146,-2.7868,1 +73.876,140.77,-0.69856,1 +6.36,-61.817,90.153,1 +31.962,129.5,81.223,1 +22.712,-9.6439,-114.41,1 +-33.39,24.978,-14.248,1 +-21.5,82.304,-59.545,1 +-54.298,0.40968,9.7717,1 +25.393,74.844,-2.1497,1 +-38.233,-72.02,73.002,1 +37.451,-29.954,-33.513,1 +-11.668,-5.9051,14.947,1 +37.368,-19.472,-23.726,1 +82.938,37.335,-49.289,1 +-27.193,-4.4609,34.933,1 +-17.499,-74.161,-52.728,1 +29.66,69.385,34.645,1 +-1.3573,4.2013,131.2,1 +-114.38,-22.365,35.86,1 +3.1298,46.251,-45.118,1 +-12.836,6.6042,-2.7868,1 +72.758,140.83,-0.69856,1 +5.4282,-61.812,90.153,1 +30.843,129.53,81.223,1 +21.732,-9.6203,-114.41,1 +-34.407,24.941,-14.248,1 +-22.583,82.279,-59.545,1 +-55.287,0.34435,9.7717,1 +24.312,74.875,-2.1497,1 +-39.126,-72.07,73.002,1 +36.505,-29.904,-33.513,1 +-12.646,-5.9217,14.947,1 +36.41,-19.418,-23.726,1 +81.896,37.459,-49.289,1 +-28.171,-4.5034,34.933,1 +-18.365,-74.189,-52.728,1 +28.562,69.434,34.645,1 +-2.3475,4.1987,131.2,1 +-115.33,-22.565,35.86,1 +2.0646,46.257,-45.118,1 +-13.829,6.5803,-2.7868,1 +71.509,140.97,-0.69856,1 +4.5683,-61.801,90.153,1 +29.604,129.59,81.223,1 +20.772,-9.5757,-114.41,1 +-35.438,24.869,-14.248,1 +-23.739,82.23,-59.545,1 +-56.265,0.22298,9.7717,1 +23.166,74.93,-2.1497,1 +-39.937,-72.159,73.002,1 +35.599,-29.818,-33.513,1 +-13.608,-5.9517,14.947,1 +35.482,-19.328,-23.726,1 +80.828,37.664,-49.289,1 +-29.134,-4.5749,34.933,1 +-19.146,-74.236,-52.728,1 +27.404,69.51,34.645,1 +-3.3318,4.1926,131.2,1 +-116.24,-22.882,35.86,1 +0.96303,46.263,-45.118,1 +-14.819,6.5413,-2.7868,1 +70.129,141.18,-0.69856,1 +3.78,-61.787,90.153,1 +28.245,129.68,81.223,1 +19.832,-9.5113,-114.41,1 +-36.484,24.76,-14.248,1 +-24.966,82.156,-59.545,1 +-57.234,0.044133,9.7717,1 +21.955,75.006,-2.1497,1 +-40.666,-72.289,73.002,1 +34.732,-29.697,-33.513,1 +-14.554,-5.9966,14.947,1 +34.583,-19.204,-23.726,1 +79.73,37.948,-49.289,1 +-30.082,-4.677,34.933,1 +-19.842,-74.303,-52.728,1 +26.187,69.611,34.645,1 +-4.3103,4.1816,131.2,1 +-117.11,-23.316,35.86,1 +-0.17479,46.267,-45.118,1 +-15.806,6.4858,-2.7868,1 +68.617,141.45,-0.69856,1 +3.0636,-61.769,90.153,1 +26.766,129.8,81.223,1 +18.911,-9.4287,-114.41,1 +-37.544,24.613,-14.248,1 +-26.265,82.055,-59.545,1 +-58.191,-0.19359,9.7717,1 +20.678,75.101,-2.1497,1 +-41.312,-72.461,73.002,1 +33.904,-29.543,-33.513,1 +-15.483,-6.0577,14.947,1 +33.713,-19.047,-23.726,1 +78.604,38.309,-49.289,1 +-31.015,-4.811,34.933,1 +-20.454,-74.391,-52.728,1 +24.909,69.734,34.645,1 +-5.2829,4.1642,131.2,1 +-117.95,-23.869,35.86,1 +-1.3489,46.269,-45.118,1 +-16.789,6.4124,-2.7868,1 +66.972,141.79,-0.69856,1 +2.4187,-61.75,90.153,1 +25.166,129.93,81.223,1 +18.009,-9.329,-114.41,1 +-38.618,24.425,-14.248,1 +-27.636,81.923,-59.545,1 +-59.138,-0.49155,9.7717,1 +19.336,75.214,-2.1497,1 +-41.874,-72.674,73.002,1 +33.115,-29.356,-33.513,1 +-16.396,-6.1363,14.947,1 +32.871,-18.859,-23.726,1 +77.447,38.746,-49.289,1 +-31.932,-4.9782,34.933,1 +-20.981,-74.499,-52.728,1 +23.571,69.879,34.645,1 +-6.2497,4.139,131.2,1 +-118.76,-24.542,35.86,1 +-2.5593,46.266,-45.118,1 +-17.768,6.3195,-2.7868,1 +65.193,142.19,-0.69856,1 +1.8455,-61.728,90.153,1 +23.446,130.09,81.223,1 +17.125,-9.2134,-114.41,1 +-39.706,24.196,-14.248,1 +-29.078,81.758,-59.545,1 +-60.072,-0.85109,9.7717,1 +17.928,75.34,-2.1497,1 +-42.353,-72.929,73.002,1 +32.364,-29.138,-33.513,1 +-17.293,-6.2336,14.947,1 +32.056,-18.639,-23.726,1 +76.258,39.256,-49.289,1 +-32.833,-5.1798,34.933,1 +-21.423,-74.629,-52.728,1 +22.173,70.042,34.645,1 +-7.2104,4.1046,131.2,1 +-119.52,-25.335,35.86,1 +-3.806,46.255,-45.118,1 +-18.742,6.2058,-2.7868,1 +63.279,142.64,-0.69856,1 +1.3439,-61.705,90.153,1 +21.605,130.26,81.223,1 +16.26,-9.0832,-114.41,1 +-40.806,23.923,-14.248,1 +-30.591,81.558,-59.545,1 +-60.992,-1.2735,9.7717,1 +16.454,75.479,-2.1497,1 +-42.746,-73.226,73.002,1 +31.651,-28.889,-33.513,1 +-18.172,-6.3508,14.947,1 +31.268,-18.389,-23.726,1 +75.036,39.837,-49.289,1 +-33.718,-5.417,34.933,1 +-21.78,-74.779,-52.728,1 +20.712,70.22,34.645,1 +-8.165,4.0597,131.2,1 +-120.24,-26.25,35.86,1 +-5.0888,46.235,-45.118,1 +-19.712,6.0698,-2.7868,1 +61.229,143.15,-0.69856,1 +0.91367,-61.681,90.153,1 +19.642,130.44,81.223,1 +15.413,-8.9393,-114.41,1 +-41.917,23.605,-14.248,1 +-32.173,81.319,-59.545,1 +-61.898,-1.76,9.7717,1 +14.913,75.626,-2.1497,1 +-43.054,-73.564,73.002,1 +30.974,-28.609,-33.513,1 +-19.035,-6.4891,14.947,1 +30.507,-18.11,-23.726,1 +73.78,40.488,-49.289,1 +-34.585,-5.691,34.933,1 +-22.05,-74.949,-52.728,1 +19.19,70.41,34.645,1 +-9.1133,4.0028,131.2,1 +-120.92,-27.287,35.86,1 +-6.4078,46.203,-45.118,1 +-20.677,5.9101,-2.7868,1 +59.042,143.71,-0.69856,1 +0.55482,-61.654,90.153,1 +17.557,130.63,81.223,1 +14.583,-8.7829,-114.41,1 +-43.04,23.239,-14.248,1 +-33.825,81.039,-59.545,1 +-62.787,-2.3118,9.7717,1 +13.305,75.778,-2.1497,1 +-43.276,-73.943,73.002,1 +30.334,-28.3,-33.513,1 +-19.879,-6.6496,14.947,1 +29.771,-17.802,-23.726,1 +72.486,41.205,-49.289,1 +-35.434,-6.0028,34.933,1 +-22.234,-75.139,-52.728,1 +17.606,70.611,34.645,1 +-10.055,3.9326,131.2,1 +-121.55,-28.446,35.86,1 +-7.7628,46.157,-45.118,1 +-21.636,5.7254,-2.7868,1 +56.715,144.3,-0.69856,1 +0.26723,-61.626,90.153,1 +15.349,130.82,81.223,1 +13.771,-8.615,-114.41,1 +-44.172,22.825,-14.248,1 +-35.544,80.713,-59.545,1 +-63.659,-2.93,9.7717,1 +11.629,75.933,-2.1497,1 +-43.409,-74.362,73.002,1 +29.728,-27.962,-33.513,1 +-20.706,-6.8332,14.947,1 +29.059,-17.468,-23.726,1 +71.154,41.986,-49.289,1 +-36.264,-6.3536,34.933,1 +-22.331,-75.348,-52.728,1 +15.959,70.817,34.645,1 +-10.99,3.8478,131.2,1 +-122.13,-29.728,35.86,1 +-9.1536,46.094,-45.118,1 +-22.589,5.5143,-2.7868,1 +54.247,144.94,-0.69856,1 +0.050762,-61.594,90.153,1 +13.019,131,81.223,1 +12.975,-8.4365,-114.41,1 +-45.312,22.359,-14.248,1 +-37.331,80.338,-59.545,1 +-64.511,-3.6157,9.7717,1 +9.8862,76.086,-2.1497,1 +-43.454,-74.82,73.002,1 +29.156,-27.595,-33.513,1 +-21.513,-7.0409,14.947,1 +28.371,-17.106,-23.726,1 +69.781,42.83,-49.289,1 +-37.074,-6.7441,34.933,1 +-22.341,-75.574,-52.728,1 +14.248,71.026,34.645,1 +-11.919,3.7471,131.2,1 +-122.66,-31.132,35.86,1 +-10.58,46.012,-45.118,1 +-23.535,5.2754,-2.7868,1 +51.636,145.6,-0.69856,1 +-0.094747,-61.559,90.153,1 +10.565,131.18,81.223,1 +12.195,-8.2483,-114.41,1 +-46.459,21.841,-14.248,1 +-39.184,79.911,-59.545,1 +-65.341,-4.3698,9.7717,1 +8.0748,76.235,-2.1497,1 +-43.409,-75.316,73.002,1 +28.617,-27.2,-33.513,1 +-22.301,-7.2736,14.947,1 +27.705,-16.718,-23.726,1 +68.364,43.732,-49.289,1 +-37.862,-7.1753,34.933,1 +-22.262,-75.816,-52.728,1 +12.474,71.234,34.645,1 +-12.84,3.6293,131.2,1 +-123.13,-32.659,35.86,1 +-12.041,45.907,-45.118,1 +-24.474,5.0074,-2.7868,1 +48.881,146.29,-0.69856,1 +-0.16949,-61.519,90.153,1 +7.9871,131.34,81.223,1 +11.431,-8.0513,-114.41,1 +-47.612,21.269,-14.248,1 +-41.102,79.427,-59.545,1 +-66.147,-5.1932,9.7717,1 +6.195,76.374,-2.1497,1 +-43.272,-75.847,73.002,1 +28.109,-26.776,-33.513,1 +-23.07,-7.5322,14.947,1 +27.06,-16.304,-23.726,1 +66.9,44.691,-49.289,1 +-38.627,-7.6481,34.933,1 +-22.094,-76.072,-52.728,1 +10.635,71.438,34.645,1 +-13.753,3.493,131.2,1 +-123.53,-34.309,35.86,1 +-13.537,45.777,-45.118,1 +-25.404,4.7091,-2.7868,1 +45.98,147,-0.69856,1 +-0.17369,-61.473,90.153,1 +5.2856,131.49,81.223,1 +10.681,-7.8463,-114.41,1 +-48.768,20.639,-14.248,1 +-43.082,78.882,-59.545,1 +-66.927,-6.0867,9.7717,1 +4.2465,76.5,-2.1497,1 +-43.042,-76.412,73.002,1 +27.632,-26.323,-33.513,1 +-23.817,-7.8174,14.947,1 +26.435,-15.865,-23.726,1 +65.388,45.702,-49.289,1 +-39.369,-8.163,34.933,1 +-21.838,-76.34,-52.728,1 +8.7318,71.632,34.645,1 +-14.658,3.337,131.2,1 +-123.87,-36.08,35.86,1 +-15.068,45.619,-45.118,1 +-26.325,4.3792,-2.7868,1 +42.931,147.72,-0.69856,1 +-0.10764,-61.42,90.153,1 +2.4604,131.6,81.223,1 +9.9461,-7.6342,-114.41,1 +-49.926,19.952,-14.248,1 +-45.123,78.272,-59.545,1 +-67.677,-7.051,9.7717,1 +2.2293,76.609,-2.1497,1 +-42.717,-77.009,73.002,1 +27.184,-25.843,-33.513,1 +-24.542,-8.1299,14.947,1 +25.829,-15.401,-23.726,1 +63.824,46.764,-49.289,1 +-40.084,-8.7209,34.933,1 +-21.491,-76.618,-52.728,1 +6.7633,71.814,34.645,1 +-15.555,3.1602,131.2,1 +-124.13,-37.973,35.86,1 +-16.632,45.43,-45.118,1 +-27.236,4.0165,-2.7868,1 +39.733,148.43,-0.69856,1 +0.028312,-61.357,90.153,1 +-0.48864,131.68,81.223,1 +9.2246,-7.4157,-114.41,1 +-51.083,19.204,-14.248,1 +-47.223,77.593,-59.545,1 +-68.395,-8.0866,9.7717,1 +0.14335,76.695,-2.1497,1 +-42.297,-77.634,73.002,1 +26.762,-25.334,-33.513,1 +-25.245,-8.4703,14.947,1 +25.24,-14.913,-23.726,1 +62.205,47.871,-49.289,1 +-40.773,-9.3221,34.933,1 +-21.054,-76.903,-52.728,1 +4.7294,71.977,34.645,1 +-16.442,2.9614,131.2,1 +-124.31,-39.985,35.86,1 +-18.229,45.206,-45.118,1 +-28.135,3.6199,-2.7868,1 +36.384,149.15,-0.69856,1 +0.23376,-61.284,90.153,1 +-3.561,131.72,81.223,1 +8.5162,-7.1915,-114.41,1 +-52.237,18.394,-14.248,1 +-49.379,76.839,-59.545,1 +-69.078,-9.1939,9.7717,1 +-2.0113,76.754,-2.1497,1 +-41.779,-78.286,73.002,1 +26.367,-24.797,-33.513,1 +-25.924,-8.8392,14.947,1 +24.666,-14.402,-23.726,1 +60.528,49.022,-49.289,1 +-41.432,-9.9671,34.933,1 +-20.526,-77.193,-52.728,1 +2.6301,72.119,34.645,1 +-17.32,2.7395,131.2,1 +-124.41,-42.115,35.86,1 +-19.857,44.945,-45.118,1 +-29.022,3.1882,-2.7868,1 +32.883,149.85,-0.69856,1 +0.50821,-61.197,90.153,1 +-6.7563,131.7,81.223,1 +7.82,-6.9624,-114.41,1 +-53.385,17.521,-14.248,1 +-51.589,76.006,-59.545,1 +-69.723,-10.373,9.7717,1 +-4.2344,76.782,-2.1497,1 +-41.162,-78.96,73.002,1 +25.996,-24.232,-33.513,1 +-26.579,-9.237,14.947,1 +24.106,-13.866,-23.726,1 +58.791,50.211,-49.289,1 diff --git a/include/steam/data/PointToPoint_distorted/poses_ic.txt b/include/steam/data/PointToPoint_distorted/poses_ic.txt new file mode 100644 index 0000000..7c796b8 --- /dev/null +++ b/include/steam/data/PointToPoint_distorted/poses_ic.txt @@ -0,0 +1,40 @@ +1,0,0,0,0,1,0,0,0,0,1,0 +1,-0.0005,0,-0.995,0.0005,1,0,-0.00024875,0,0,1,0 +1,-0.002,0,-1.98,0.002,1,0,-0.00198,0,0,1,0 +0.99999,-0.0045,0,-2.955,0.0045,0.99999,0,-0.0066487,0,0,1,0 +0.99997,-0.0079999,0,-3.92,0.0079999,0.99997,0,-0.01568,0,0,1,0 +0.99992,-0.0125,0,-4.8749,0.0125,0.99992,0,-0.030468,0,0,1,0 +0.99984,-0.017999,0,-5.8197,0.017999,0.99984,0,-0.052379,0,0,1,0 +0.9997,-0.024498,0,-6.7543,0.024498,0.9997,0,-0.082745,0,0,1,0 +0.99949,-0.031995,0,-7.6787,0.031995,0.99949,0,-0.12287,0,0,1,0 +0.99918,-0.040489,0,-8.5927,0.040489,0.99918,0,-0.17402,0,0,1,0 +0.99875,-0.049979,0,-9.496,0.049979,0.99875,0,-0.23745,0,0,1,0 +0.99817,-0.060463,0,-10.389,0.060463,0.99817,0,-0.31435,0,0,1,0 +0.99741,-0.071938,0,-11.27,0.071938,0.99741,0,-0.4059,0,0,1,0 +0.99643,-0.084399,0,-12.141,0.084399,0.99643,0,-0.51324,0,0,1,0 +0.9952,-0.097843,0,-12.999,0.097843,0.9952,0,-0.63747,0,0,1,0 +0.99368,-0.11226,0,-13.846,0.11226,0.99368,0,-0.77965,0,0,1,0 +0.99182,-0.12765,0,-14.68,0.12765,0.99182,0,-0.94079,0,0,1,0 +0.98958,-0.144,0,-15.501,0.144,0.98958,0,-1.1219,0,0,1,0 +0.98691,-0.16129,0,-16.308,0.16129,0.98691,0,-1.3239,0,0,1,0 +0.98375,-0.17952,0,-17.102,0.17952,0.98375,0,-1.5476,0,0,1,0 +-10,0,0,0,0,0,1,0,0,0,0,0.1 +-9.9,0,0,0,0,0.01,1,0,0,0,0,0.1 +-9.8,0,0,0,0,0.02,1,0,0,0,0,0.1 +-9.7,0,0,0,0,0.03,1,0,0,0,0,0.1 +-9.6,0,0,0,0,0.04,1,0,0,0,0,0.1 +-9.5,0,0,0,0,0.05,1,0,0,0,0,0.1 +-9.4,0,0,0,0,0.06,1,0,0,0,0,0.1 +-9.3,0,0,0,0,0.07,1,0,0,0,0,0.1 +-9.2,0,0,0,0,0.08,1,0,0,0,0,0.1 +-9.1,0,0,0,0,0.09,1,0,0,0,0,0.1 +-9,0,0,0,0,0.1,1,0,0,0,0,0.1 +-8.9,0,0,0,0,0.11,1,0,0,0,0,0.1 +-8.8,0,0,0,0,0.12,1,0,0,0,0,0.1 +-8.7,0,0,0,0,0.13,1,0,0,0,0,0.1 +-8.6,0,0,0,0,0.14,1,0,0,0,0,0.1 +-8.5,0,0,0,0,0.15,1,0,0,0,0,0.1 +-8.4,0,0,0,0,0.16,1,0,0,0,0,0.1 +-8.3,0,0,0,0,0.17,1,0,0,0,0,0.1 +-8.2,0,0,0,0,0.18,1,0,0,0,0,0.1 +-8.1,0,0,0,0,0.19,1,0,0,0,0,0.1 diff --git a/include/steam/data/points_simulated.txt b/include/steam/data/points_simulated.txt new file mode 100644 index 0000000..e3ead1f --- /dev/null +++ b/include/steam/data/points_simulated.txt @@ -0,0 +1,400 @@ +-26.151,-4.4462,34.933,1 +-16.45,-74.15,-52.728,1 +30.849,69.365,34.645,1 +-0.16151,4.2019,131.2,1 +-113.16,-22.276,35.86,1 +4.4604,46.248,-45.118,1 +-11.489,6.6153,-2.7868,1 +74.286,140.76,-0.69856,1 +6.8027,-61.818,90.153,1 +32.477,129.5,81.223,1 +23.259,-9.6475,-114.41,1 +-32.788,24.984,-14.248,1 +-20.835,82.308,-59.545,1 +-53.601,0.42286,9.7717,1 +26.161,74.836,-2.1497,1 +-37.459,-72.008,73.002,1 +38.287,-29.968,-33.513,1 +-10.775,-5.9006,14.947,1 +38.304,-19.489,-23.726,1 +83.951,37.293,-49.289,1 +-27.129,-4.4862,34.933,1 +-17.324,-74.176,-52.728,1 +29.76,69.411,34.645,1 +-1.1528,4.2009,131.2,1 +-114.11,-22.446,35.86,1 +3.4061,46.254,-45.118,1 +-12.484,6.5973,-2.7868,1 +73.09,140.87,-0.69856,1 +5.9105,-61.809,90.153,1 +31.298,129.55,81.223,1 +22.288,-9.6133,-114.41,1 +-33.81,24.934,-14.248,1 +-21.943,82.276,-59.545,1 +-54.586,0.34172,9.7717,1 +25.064,74.875,-2.1497,1 +-38.336,-72.065,73.002,1 +37.347,-29.911,-33.513,1 +-11.751,-5.9175,14.947,1 +37.349,-19.432,-23.726,1 +82.91,37.418,-49.289,1 +-28.064,-4.6089,34.933,1 +-17.945,-74.253,-52.728,1 +28.492,69.543,34.645,1 +-2.1267,4.195,131.2,1 +-114.96,-22.96,35.86,1 +2.2429,46.268,-45.118,1 +-13.468,6.5404,-2.7868,1 +71.5,141.2,-0.69856,1 +5.2335,-61.782,90.153,1 +29.76,129.68,81.223,1 +21.376,-9.5135,-114.41,1 +-34.877,24.781,-14.248,1 +-23.268,82.176,-59.545,1 +-55.542,0.095427,9.7717,1 +23.772,74.986,-2.1497,1 +-38.966,-72.237,73.002,1 +36.526,-29.744,-33.513,1 +-12.679,-5.971,14.947,1 +36.481,-19.265,-23.726,1 +81.786,37.79,-49.289,1 +-28.953,-4.8182,34.933,1 +-18.313,-74.385,-52.728,1 +27.045,69.756,34.645,1 +-3.0831,4.18,131.2,1 +-115.71,-23.821,35.86,1 +0.9708,46.285,-45.118,1 +-14.442,6.4402,-2.7868,1 +69.514,141.73,-0.69856,1 +4.7717,-61.74,90.153,1 +27.861,129.9,81.223,1 +20.522,-9.3519,-114.41,1 +-35.987,24.52,-14.248,1 +-24.809,82,-59.545,1 +-56.466,-0.32011,9.7717,1 +22.284,75.163,-2.1497,1 +-39.348,-72.526,73.002,1 +35.823,-29.468,-33.513,1 +-13.559,-6.0649,14.947,1 +35.699,-18.989,-23.726,1 +80.575,38.404,-49.289,1 +-29.796,-5.1176,34.933,1 +-18.426,-74.569,-52.728,1 +25.416,70.041,34.645,1 +-4.0219,4.1517,131.2,1 +-116.35,-25.03,35.86,1 +-0.41032,46.297,-45.118,1 +-15.404,6.2926,-2.7868,1 +67.127,142.46,-0.69856,1 +4.5246,-61.682,90.153,1 +25.601,130.19,81.223,1 +19.724,-9.1316,-114.41,1 +-37.137,24.145,-14.248,1 +-26.564,81.74,-59.545,1 +-57.355,-0.90867,9.7717,1 +20.598,75.397,-2.1497,1 +-39.48,-72.931,73.002,1 +35.235,-29.086,-33.513,1 +-14.39,-6.2026,14.947,1 +35.001,-18.609,-23.726,1 +79.272,39.252,-49.289,1 +-30.59,-5.5102,34.933,1 +-18.283,-74.802,-52.728,1 +23.603,70.387,34.645,1 +-4.9428,4.1062,131.2,1 +-116.87,-26.589,35.86,1 +-1.9005,46.296,-45.118,1 +-16.353,6.0932,-2.7868,1 +64.333,143.36,-0.69856,1 +4.4917,-61.606,90.153,1 +22.976,130.54,81.223,1 +18.98,-8.8554,-114.41,1 +-38.325,23.651,-14.248,1 +-28.53,81.383,-59.545,1 +-58.203,-1.6737,9.7717,1 +18.713,75.678,-2.1497,1 +-39.357,-73.448,73.002,1 +34.759,-28.598,-33.513,1 +-15.17,-6.3871,14.947,1 +34.384,-18.126,-23.726,1 +77.87,40.328,-49.289,1 +-31.33,-5.9986,34.933,1 +-17.881,-75.077,-52.728,1 +21.603,70.782,34.645,1 +-5.8454,4.0397,131.2,1 +-117.25,-28.498,35.86,1 +-3.4996,46.274,-45.118,1 +-17.286,5.8382,-2.7868,1 +61.123,144.42,-0.69856,1 +4.672,-61.508,90.153,1 +19.983,130.91,81.223,1 +18.288,-8.5254,-114.41,1 +-39.546,23.031,-14.248,1 +-30.704,80.916,-59.545,1 +-59.003,-2.6182,9.7717,1 +16.627,75.992,-2.1497,1 +-38.975,-74.072,73.002,1 +34.391,-28.005,-33.513,1 +-15.898,-6.621,14.947,1 +33.843,-17.54,-23.726,1 +76.359,41.622,-49.289,1 +-32.013,-6.5847,34.933,1 +-17.22,-75.388,-52.728,1 +19.413,71.213,34.645,1 +-6.729,3.9485,131.2,1 +-117.48,-30.756,35.86,1 +-5.2071,46.221,-45.118,1 +-18.203,5.5236,-2.7868,1 +57.49,145.61,-0.69856,1 +5.0646,-61.382,90.153,1 +16.621,131.3,81.223,1 +17.645,-8.1436,-114.41,1 +-40.793,22.279,-14.248,1 +-33.082,80.326,-59.545,1 +-59.746,-3.7445,9.7717,1 +14.336,76.325,-2.1497,1 +-38.329,-74.794,73.002,1 +34.125,-27.306,-33.513,1 +-16.572,-6.9061,14.947,1 +33.373,-16.854,-23.726,1 +74.727,43.127,-49.289,1 +-32.634,-7.2701,34.933,1 +-16.296,-75.723,-52.728,1 +17.029,71.665,34.645,1 +-7.5927,3.8294,131.2,1 +-117.53,-33.358,35.86,1 +-7.0223,46.125,-45.118,1 +-19.099,5.1459,-2.7868,1 +53.423,146.9,-0.69856,1 +5.6677,-61.219,90.153,1 +12.886,131.68,81.223,1 +17.047,-7.7113,-114.41,1 +-42.061,21.389,-14.248,1 +-35.658,79.595,-59.545,1 +-60.423,-5.0545,9.7717,1 +11.839,76.662,-2.1497,1 +-37.414,-75.604,73.002,1 +33.954,-26.498,-33.513,1 +-17.189,-7.2439,14.947,1 +32.967,-16.065,-23.726,1 +72.961,44.831,-49.289,1 +-33.185,-8.0553,34.933,1 +-15.108,-76.07,-52.728,1 +14.449,72.121,34.645,1 +-8.4355,3.679,131.2,1 +-117.39,-36.299,35.86,1 +-8.9436,45.975,-45.118,1 +-19.972,4.7017,-2.7868,1 +48.912,148.25,-0.69856,1 +6.4792,-61.01,90.153,1 +8.7767,132.01,81.223,1 +16.49,-7.2297,-114.41,1 +-43.34,20.354,-14.248,1 +-38.423,78.704,-59.545,1 +-61.023,-6.5491,9.7717,1 +9.1325,76.983,-2.1497,1 +-36.221,-76.489,73.002,1 +33.871,-25.579,-33.513,1 +-17.746,-7.6354,14.947,1 +32.618,-15.175,-23.726,1 +71.046,46.721,-49.289,1 +-33.661,-8.9406,34.933,1 +-13.653,-76.412,-52.728,1 +11.669,72.56,34.645,1 +-9.2558,3.4943,131.2,1 +-117.03,-39.572,35.86,1 +-10.969,45.759,-45.118,1 +-20.816,4.1878,-2.7868,1 +43.949,149.65,-0.69856,1 +7.4962,-60.744,90.153,1 +4.2926,132.26,81.223,1 +15.971,-6.6997,-114.41,1 +-44.621,19.168,-14.248,1 +-41.369,77.634,-59.545,1 +-61.53,-8.2282,9.7717,1 +6.2161,77.269,-2.1497,1 +-34.746,-77.433,73.002,1 +33.867,-24.546,-33.513,1 +-18.24,-8.0808,14.947,1 +32.319,-14.182,-23.726,1 +68.967,48.784,-49.289,1 +-34.054,-9.9248,34.933,1 +-11.93,-76.733,-52.728,1 +8.6869,72.963,34.645,1 +-10.052,3.2725,131.2,1 +-116.41,-43.167,35.86,1 +-13.095,45.462,-45.118,1 +-21.629,3.6016,-2.7868,1 +38.523,151.03,-0.69856,1 +8.7148,-60.406,90.153,1 +-0.5658,132.4,81.223,1 +15.483,-6.122,-114.41,1 +-45.893,17.824,-14.248,1 +-44.484,76.364,-59.545,1 +-61.931,-10.091,9.7717,1 +3.0887,77.498,-2.1497,1 +-32.981,-78.418,73.002,1 +33.932,-23.396,-33.513,1 +-18.667,-8.5799,14.947,1 +32.058,-13.086,-23.726,1 +66.706,51.003,-49.289,1 +-34.354,-11.006,34.933,1 +-9.9394,-77.011,-52.728,1 +5.5019,73.307,34.645,1 +-10.822,3.011,131.2,1 +-115.52,-47.07,35.86,1 +-15.319,45.071,-45.118,1 +-22.403,2.9407,-2.7868,1 +32.628,152.35,-0.69856,1 +10.13,-59.982,90.153,1 +-5.7955,132.39,81.223,1 +15.022,-5.497,-114.41,1 +-47.144,16.318,-14.248,1 +-47.755,74.872,-59.545,1 +-62.209,-12.133,9.7717,1 +-0.24937,77.646,-2.1497,1 +-30.919,-79.421,73.002,1 +34.056,-22.125,-33.513,1 +-19.024,-9.1314,14.947,1 +31.828,-11.885,-23.726,1 +64.244,53.361,-49.289,1 +-34.553,-12.182,34.933,1 +-7.6813,-77.225,-52.728,1 +2.1135,73.566,34.645,1 +-11.563,2.7077,131.2,1 +-114.31,-51.263,35.86,1 +-17.634,44.569,-45.118,1 +-23.134,2.2032,-2.7868,1 +26.257,153.57,-0.69856,1 +11.736,-59.456,90.153,1 +-11.391,132.18,81.223,1 +14.581,-4.8254,-114.41,1 +-48.359,14.644,-14.248,1 +-51.165,73.133,-59.545,1 +-62.347,-14.353,9.7717,1 +-3.7964,77.686,-2.1497,1 +-28.556,-80.42,73.002,1 +34.226,-20.728,-33.513,1 +-19.304,-9.7337,14.947,1 +31.615,-10.579,-23.726,1 +61.562,55.836,-49.289,1 +-34.642,-13.448,34.933,1 +-5.1583,-77.35,-52.728,1 +-1.4768,73.715,34.645,1 +-12.274,2.3606,131.2,1 +-112.75,-55.726,35.86,1 +-20.034,43.942,-45.118,1 +-23.814,1.3881,-2.7868,1 +19.407,154.63,-0.69856,1 +13.523,-58.809,90.153,1 +-17.344,131.74,81.223,1 +14.155,-4.1077,-114.41,1 +-49.522,12.797,-14.248,1 +-54.694,71.125,-59.545,1 +-62.325,-16.742,9.7717,1 +-7.5487,77.592,-2.1497,1 +-25.886,-81.387,73.002,1 +34.427,-19.202,-33.513,1 +-19.504,-10.384,14.947,1 +31.407,-9.1669,-23.726,1 +58.641,58.406,-49.289,1 +-34.609,-14.798,34.933,1 +-2.3744,-77.357,-52.728,1 +-5.2657,73.724,34.645,1 +-12.949,1.9682,131.2,1 +-110.81,-60.433,35.86,1 +-22.511,43.173,-45.118,1 +-24.437,0.49467,-2.7868,1 +12.08,155.48,-0.69856,1 +15.483,-58.021,90.153,1 +-23.641,131,81.223,1 +13.735,-3.3448,-114.41,1 +-50.616,10.775,-14.248,1 +-58.32,68.823,-59.545,1 +-62.122,-19.293,9.7717,1 +-11.5,77.334,-2.1497,1 +-22.907,-82.292,73.002,1 +34.644,-17.543,-33.513,1 +-19.619,-11.079,14.947,1 +31.191,-7.6489,-23.726,1 +55.461,61.045,-49.289,1 +-34.444,-16.226,34.933,1 +0.66351,-77.219,-52.728,1 +-9.2474,73.565,34.645,1 +-13.587,1.5292,131.2,1 +-108.44,-65.353,35.86,1 +-25.054,42.245,-45.118,1 +-24.994,-0.47669,-2.7868,1 +4.2791,156.03,-0.69856,1 +17.603,-57.074,90.153,1 +-30.265,129.93,81.223,1 +13.316,-2.5376,-114.41,1 +-51.623,8.5754,-14.248,1 +-62.016,66.203,-59.545,1 +-61.719,-21.995,9.7717,1 +-15.643,76.881,-2.1497,1 +-19.618,-83.103,73.002,1 +34.862,-15.748,-33.513,1 +-19.643,-11.814,14.947,1 +30.953,-6.0257,-23.726,1 +52.003,63.722,-49.289,1 +-34.136,-17.722,34.933,1 +3.9462,-76.904,-52.728,1 +-13.413,73.205,34.645,1 +-14.184,1.043,131.2,1 +-105.61,-70.451,35.86,1 +-27.651,41.141,-45.118,1 +-25.477,-1.5249,-2.7868,1 +-3.984,156.24,-0.69856,1 +19.868,-55.945,90.153,1 +-37.194,128.46,81.223,1 +12.888,-1.6878,-114.41,1 +-52.521,6.1985,-14.248,1 +-65.753,63.241,-59.545,1 +-61.092,-24.834,9.7717,1 +-19.965,76.201,-2.1497,1 +-16.019,-83.784,73.002,1 +35.061,-13.816,-33.513,1 +-19.572,-12.583,14.947,1 +30.676,-4.2989,-23.726,1 +48.249,66.405,-49.289,1 +-33.675,-19.277,34.933,1 +7.4608,-76.379,-52.728,1 +-17.752,72.612,34.645,1 +-14.734,0.50924,131.2,1 +-102.28,-75.683,35.86,1 +-30.287,39.846,-45.118,1 +-25.877,-2.6477,-2.7868,1 +-12.693,156.03,-0.69856,1 +22.261,-54.613,90.153,1 +-44.399,126.54,81.223,1 +12.443,-0.79713,-114.41,1 +-53.29,3.6458,-14.248,1 +-69.497,59.915,-59.545,1 +-60.22,-27.793,9.7717,1 +-24.452,75.26,-2.1497,1 +-12.116,-84.297,73.002,1 +35.222,-11.745,-33.513,1 +-19.4,-13.381,14.947,1 +30.343,-2.4712,-23.726,1 +44.182,69.057,-49.289,1 +-33.049,-20.878,34.933,1 +11.191,-75.61,-52.728,1 +-22.247,71.753,34.645,1 +-15.235,-0.071632,131.2,1 +-98.42,-81.003,35.86,1 +-32.946,38.342,-45.118,1 +-26.185,-3.8418,-2.7868,1 +-21.824,155.32,-0.69856,1 +24.761,-53.057,90.153,1 +-51.846,124.12,81.223,1 +11.973,0.13157,-114.41,1 +-53.905,0.92141,-14.248,1 +-73.209,56.205,-59.545,1 +-59.081,-30.854,9.7717,1 +-29.084,74.025,-2.1497,1 +-7.9164,-84.602,73.002,1 +35.325,-9.5362,-33.513,1 +-19.123,-14.199,14.947,1 +29.939,-0.54698,-23.726,1 +39.788,71.639,-49.289,1 diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 65d6d87..a579335 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -47,4 +47,8 @@ target_link_libraries(SimpleBAandCATrajPrior steam ${DEPEND_LIBS}) # simple example of constant acceleration trajectory interface for prior, combined with p2p error terms add_executable(SimpleP2PandCATrajPrior ${CMAKE_CURRENT_SOURCE_DIR}/SimpleP2PandCATrajPrior.cpp) -target_link_libraries(SimpleP2PandCATrajPrior steam ${DEPEND_LIBS}) \ No newline at end of file +target_link_libraries(SimpleP2PandCATrajPrior steam ${DEPEND_LIBS}) + +# simple example of constant acceleration trajectory interface for prior, combined with p2p error terms +add_executable(MotionDistortedP2PandCATrajPrior ${CMAKE_CURRENT_SOURCE_DIR}/MotionDistortedP2PandCATrajPrior.cpp) +target_link_libraries(MotionDistortedP2PandCATrajPrior steam ${DEPEND_LIBS}) diff --git a/samples/MotionDistortedP2PandCATrajPrior.cpp b/samples/MotionDistortedP2PandCATrajPrior.cpp new file mode 100644 index 0000000..e77bb96 --- /dev/null +++ b/samples/MotionDistortedP2PandCATrajPrior.cpp @@ -0,0 +1,236 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file MotionDistortedP2PandCATrajPrior.cpp +/// \brief A sample usage of the STEAM Engine library for a bundle adjustment problem +/// with point-to-point error terms and trajectory smoothing factors. +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include +#include +#include +#include + +using namespace std; +typedef steam::PointToPointErrorEval Error; +typedef steam::WeightedLeastSqCostTerm<4,6> Cost; +unsigned int M = 20; +unsigned int N = 20; + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Structure to store trajectory state variables +////////////////////////////////////////////////////////////////////////////////////////////// +struct TrajStateVar { + steam::Time time; + steam::se3::TransformStateVar::Ptr pose; + steam::VectorSpaceStateVar::Ptr velocity; + steam::VectorSpaceStateVar::Ptr acceleration; +}; + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Example that loads and solves simple bundle adjustment problems +////////////////////////////////////////////////////////////////////////////////////////////// +int main(int argc, char** argv) { + + /// + /// Setup Traj Prior + /// + + + // Smoothing factor diagonal -- in this example, we penalize accelerations in each dimension + // except for the forward and yaw (this should be fairly typical) + double lin_acc_stddev_x = 1.00; // body-centric (e.g. x is usually forward) + double lin_acc_stddev_y = 0.01; // body-centric (e.g. y is usually side-slip) + double lin_acc_stddev_z = 0.01; // body-centric (e.g. z is usually 'jump') + double ang_acc_stddev_x = 0.01; // ~roll + double ang_acc_stddev_y = 0.01; // ~pitch + double ang_acc_stddev_z = 1.00; // ~yaw + Eigen::Array Qc_diag; + Qc_diag << lin_acc_stddev_x, lin_acc_stddev_y, lin_acc_stddev_z, + ang_acc_stddev_x, ang_acc_stddev_y, ang_acc_stddev_z; + Qc_diag = Qc_diag * 1; + + // Make Qc_inv + Eigen::Matrix Qc_inv; Qc_inv.setZero(); + Qc_inv.diagonal() = 1.0/Qc_diag; + + /// + /// Parse Dataset + /// + + string filePath = "../../include/steam/data/PointToPoint_distorted/points_simulated.txt"; + ifstream in(filePath); + + vector> fields; + int count = 0; + if (in) { + string line; + while (getline(in, line)) { + count++; + stringstream sep(line); + string field; + fields.push_back(vector()); + while (getline(sep, field, ',')) { + fields.back().push_back(stod(field)); + } + } + } + + filePath = "../../include/steam/data/PointToPoint_distorted/poses_ic.txt"; + ifstream inIc(filePath); + + vector> fieldsIC; + count = 0; + if (inIc) { + string line; + while (getline(inIc, line)) { + count++; + stringstream sep(line); + string field; + fieldsIC.push_back(vector()); + while (getline(sep, field, ',')) { + fieldsIC.back().push_back(stod(field)); + } + } + } + + cout << fieldsIC.size() << endl; + cout << fieldsIC.back().front() << endl; + + // State variable containers (and related data) + std::vector traj_states_ic; + + // Zero velocity + Eigen::Matrix initVelocity; initVelocity.setZero(); + // initVelocity << -10.0, 0, 0, 0, 0, 0; + + // Zero acceleration + Eigen::Matrix initAcceleration; initAcceleration.setZero(); + // initAcceleration << -3.0, 0, 0, 0, 0, 0; + + // Setup state variables using initial condition + for (unsigned int i = 0; i < N; i++) { + TrajStateVar temp; + double dt = double(i-0)*0.1; + temp.time = steam::Time(dt); + Eigen::Matrix T_k0; + T_k0 << fieldsIC[i][0], fieldsIC[i][1], fieldsIC[i][2], fieldsIC[i][3], + fieldsIC[i][4], fieldsIC[i][5], fieldsIC[i][6], fieldsIC[i][7], + fieldsIC[i][8], fieldsIC[i][9], fieldsIC[i][10], fieldsIC[i][11], + 0.0, 0.0, 0.0, 1; + + lgmath::se3::Transformation T_k0_tf(T_k0); + + temp.pose = steam::se3::TransformStateVar::Ptr(new steam::se3::TransformStateVar(T_k0_tf)); + // initVelocity << fieldsIC[i+20][0],fieldsIC[i+20][1],fieldsIC[i+20][2],fieldsIC[i+20][3],fieldsIC[i+20][4],fieldsIC[i+20][5]; + // initAcceleration << fieldsIC[i+20][6],fieldsIC[i+20][7],fieldsIC[i+20][8],fieldsIC[i+20][9],fieldsIC[i+20][10],fieldsIC[i+20][11]; + + temp.velocity = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initVelocity)); + temp.acceleration = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initAcceleration)); + traj_states_ic.push_back(temp); + } + + // Setup Trajectory + steam::se3::SteamCATrajInterface traj(Qc_inv); + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + TrajStateVar& state = traj_states_ic.at(i); + steam::se3::TransformStateEvaluator::Ptr temp = + steam::se3::TransformStateEvaluator::MakeShared(state.pose); + traj.add(state.time, temp, state.velocity, state.acceleration); + } + + // Lock first pose (otherwise entire solution is 'floating') + // **Note: alternatively we could add a prior to the first pose. + traj_states_ic[0].pose->setLock(true); + + + // steam cost terms + steam::ParallelizedCostTermCollection::Ptr measCostTerms(new steam::ParallelizedCostTermCollection()); + + // Setup shared noise and loss function + Eigen::Matrix4d measurementNoise; + measurementNoise.setIdentity(); + steam::BaseNoiseModel<4>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<4>(measurementNoise)); + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + Error::Ptr error; + Cost::Ptr cost; + + // cout << fields[1*N+1][1] << endl; + + for (unsigned int i = 1; i < N; i++) { + // steam::se3::TransformEvaluator::Ptr T_k0 = steam::se3::TransformStateEvaluator::MakeShared(traj_states_ic[i].pose); + + for (unsigned int j = 0; j < M; j++) { + double t = (i-1)*0.1 + double(j+1)/double(M)*0.1; + auto T_k0 = traj.getInterpPoseEval(t); + Eigen::Vector4d ref_a; + ref_a << fields[j][0] , fields[j][1] , fields[j][2] , fields[j][3]; + + Eigen::Vector4d read_b; + read_b << fields[i*N+j][0] , fields[i*N+j][1] , fields[i*N+j][2] , fields[i*N+j][3]; + + error.reset(new Error(ref_a, read_b, T_k0)); + cost.reset(new Cost(error, sharedNoiseModel, sharedLossFunc)); + measCostTerms->add(cost); + } + } + + // Trajectory prior smoothing terms + steam::ParallelizedCostTermCollection::Ptr smoothingCostTerms(new steam::ParallelizedCostTermCollection()); + traj.appendPriorCostTerms(smoothingCostTerms); + + /// + /// Make Optimization Problem + /// + + // Initialize problem + steam::OptimizationProblem problem; + + // Add state variables + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + const TrajStateVar& state = traj_states_ic.at(i); + problem.addStateVariable(state.pose); + problem.addStateVariable(state.velocity); + problem.addStateVariable(state.acceleration); + } + + // Add cost terms + problem.addCostTerm(measCostTerms); + problem.addCostTerm(smoothingCostTerms); + + /// + /// Setup Solver and Optimize + /// + typedef steam::VanillaGaussNewtonSolver SolverType; + + // Initialize parameters (enable verbose mode) + SolverType::Params params; + params.verbose = true; + params.absoluteCostChangeThreshold = 1e-20; + params.relativeCostChangeThreshold = 1e-20; + + // Make solver + SolverType solver(&problem, params); + + // Optimize + TrajStateVar& state = traj_states_ic.back(); + cout << state.velocity->getValue().transpose() << endl; + cout << state.acceleration->getValue().transpose() << endl; + + solver.optimize(); + + // Setup Trajectory + // for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + // TrajStateVar& state = traj_states_ic.at(i); + // std::cout << i << ": \n " << state.pose->getValue() << "\n"; + // } + + state = traj_states_ic.back(); + cout << state.pose->getValue() << endl; + cout << state.velocity->getValue().transpose() << endl; + cout << state.acceleration->getValue().transpose() << endl; + return 0; + +} diff --git a/samples/SimpleP2PandCATrajPrior.cpp b/samples/SimpleP2PandCATrajPrior.cpp new file mode 100644 index 0000000..2a386ba --- /dev/null +++ b/samples/SimpleP2PandCATrajPrior.cpp @@ -0,0 +1,235 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SimpleP2PandCATrajPrior.cpp +/// \brief A sample usage of the STEAM Engine library for a bundle adjustment problem +/// with point-to-point error terms and trajectory smoothing factors. +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include +#include +#include +#include + +using namespace std; +typedef steam::PointToPointErrorEval Error; +typedef steam::WeightedLeastSqCostTerm<4,6> Cost; +unsigned int M = 20; +unsigned int N = 20; + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Structure to store trajectory state variables +////////////////////////////////////////////////////////////////////////////////////////////// +struct TrajStateVar { + steam::Time time; + steam::se3::TransformStateVar::Ptr pose; + steam::VectorSpaceStateVar::Ptr velocity; + steam::VectorSpaceStateVar::Ptr acceleration; +}; + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Example that loads and solves simple bundle adjustment problems +////////////////////////////////////////////////////////////////////////////////////////////// +int main(int argc, char** argv) { + + /// + /// Setup Traj Prior + /// + + + // Smoothing factor diagonal -- in this example, we penalize accelerations in each dimension + // except for the forward and yaw (this should be fairly typical) + double lin_acc_stddev_x = 1.00; // body-centric (e.g. x is usually forward) + double lin_acc_stddev_y = 0.01; // body-centric (e.g. y is usually side-slip) + double lin_acc_stddev_z = 0.01; // body-centric (e.g. z is usually 'jump') + double ang_acc_stddev_x = 0.01; // ~roll + double ang_acc_stddev_y = 0.01; // ~pitch + double ang_acc_stddev_z = 1.00; // ~yaw + Eigen::Array Qc_diag; + Qc_diag << lin_acc_stddev_x, lin_acc_stddev_y, lin_acc_stddev_z, + ang_acc_stddev_x, ang_acc_stddev_y, ang_acc_stddev_z; + Qc_diag = Qc_diag * 1; + + // Make Qc_inv + Eigen::Matrix Qc_inv; Qc_inv.setZero(); + Qc_inv.diagonal() = 1.0/Qc_diag; + + /// + /// Parse Dataset + /// + + string filePath = "../../include/steam/data/PointToPoint/points_simulated.txt"; + ifstream in(filePath); + + vector> fields; + int count = 0; + if (in) { + string line; + while (getline(in, line)) { + count++; + stringstream sep(line); + string field; + fields.push_back(vector()); + while (getline(sep, field, ',')) { + fields.back().push_back(stod(field)); + } + } + } + + filePath = "../../include/steam/data/PointToPoint/poses_ic.txt"; + ifstream inIc(filePath); + + vector> fieldsIC; + count = 0; + if (inIc) { + string line; + while (getline(inIc, line)) { + count++; + stringstream sep(line); + string field; + fieldsIC.push_back(vector()); + while (getline(sep, field, ',')) { + fieldsIC.back().push_back(stod(field)); + } + } + } + + cout << fieldsIC.size() << endl; + cout << fieldsIC.back().front() << endl; + + // State variable containers (and related data) + std::vector traj_states_ic; + + // Zero velocity + Eigen::Matrix initVelocity; initVelocity.setZero(); + // initVelocity << -10.0, 0, 0, 0, 0, 0; + + // Zero acceleration + Eigen::Matrix initAcceleration; initAcceleration.setZero(); + // initAcceleration << -3.0, 0, 0, 0, 0, 0; + + // Setup state variables using initial condition + for (unsigned int i = 0; i < N; i++) { + TrajStateVar temp; + double dt = double(i-0)*0.1; + temp.time = steam::Time(dt); + Eigen::Matrix T_k0; + T_k0 << fieldsIC[i][0], fieldsIC[i][1], fieldsIC[i][2], fieldsIC[i][3], + fieldsIC[i][4], fieldsIC[i][5], fieldsIC[i][6], fieldsIC[i][7], + fieldsIC[i][8], fieldsIC[i][9], fieldsIC[i][10], fieldsIC[i][11], + 0.0, 0.0, 0.0, 1; + + lgmath::se3::Transformation T_k0_tf(T_k0); + + temp.pose = steam::se3::TransformStateVar::Ptr(new steam::se3::TransformStateVar(T_k0_tf)); + initVelocity << fieldsIC[i+20][0],fieldsIC[i+20][1],fieldsIC[i+20][2],fieldsIC[i+20][3],fieldsIC[i+20][4],fieldsIC[i+20][5]; + initAcceleration << fieldsIC[i+20][6],fieldsIC[i+20][7],fieldsIC[i+20][8],fieldsIC[i+20][9],fieldsIC[i+20][10],fieldsIC[i+20][11]; + cout << initVelocity.transpose() << endl; + + temp.velocity = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initVelocity)); + temp.acceleration = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initAcceleration)); + traj_states_ic.push_back(temp); + } + + // Setup Trajectory + steam::se3::SteamCATrajInterface traj(Qc_inv); + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + TrajStateVar& state = traj_states_ic.at(i); + steam::se3::TransformStateEvaluator::Ptr temp = + steam::se3::TransformStateEvaluator::MakeShared(state.pose); + traj.add(state.time, temp, state.velocity, state.acceleration); + } + + // Lock first pose (otherwise entire solution is 'floating') + // **Note: alternatively we could add a prior to the first pose. + traj_states_ic[0].pose->setLock(true); + + + // steam cost terms + steam::ParallelizedCostTermCollection::Ptr measCostTerms(new steam::ParallelizedCostTermCollection()); + + // Setup shared noise and loss function + Eigen::Matrix4d measurementNoise; + measurementNoise.setIdentity(); + steam::BaseNoiseModel<4>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<4>(measurementNoise)); + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + Error::Ptr error; + Cost::Ptr cost; + + // cout << fields[1*N+1][1] << endl; + + for (unsigned int i = 1; i < N; i++) { + steam::se3::TransformEvaluator::Ptr T_k0 = steam::se3::TransformStateEvaluator::MakeShared(traj_states_ic[i].pose); + + for (unsigned int j = 0; j < M; j++) { + Eigen::Vector4d ref_a; + ref_a << fields[j][0] , fields[j][1] , fields[j][2] , fields[j][3]; + + Eigen::Vector4d read_b; + read_b << fields[i*N+j][0] , fields[i*N+j][1] , fields[i*N+j][2] , fields[i*N+j][3]; + + error.reset(new Error(ref_a, read_b, T_k0)); + cost.reset(new Cost(error, sharedNoiseModel, sharedLossFunc)); + measCostTerms->add(cost); + } + } + + // Trajectory prior smoothing terms + steam::ParallelizedCostTermCollection::Ptr smoothingCostTerms(new steam::ParallelizedCostTermCollection()); + traj.appendPriorCostTerms(smoothingCostTerms); + + /// + /// Make Optimization Problem + /// + + // Initialize problem + steam::OptimizationProblem problem; + + // Add state variables + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + const TrajStateVar& state = traj_states_ic.at(i); + problem.addStateVariable(state.pose); + problem.addStateVariable(state.velocity); + problem.addStateVariable(state.acceleration); + } + + // Add cost terms + problem.addCostTerm(measCostTerms); + problem.addCostTerm(smoothingCostTerms); + + /// + /// Setup Solver and Optimize + /// + typedef steam::VanillaGaussNewtonSolver SolverType; + + // Initialize parameters (enable verbose mode) + SolverType::Params params; + params.verbose = true; + params.absoluteCostChangeThreshold = 1e-20; + params.relativeCostChangeThreshold = 1e-20; + + // Make solver + SolverType solver(&problem, params); + + // Optimize + TrajStateVar& state = traj_states_ic.back(); + cout << state.velocity->getValue().transpose() << endl; + cout << state.acceleration->getValue().transpose() << endl; + + solver.optimize(); + + // Setup Trajectory + // for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + // TrajStateVar& state = traj_states_ic.at(i); + // std::cout << i << ": \n " << state.pose->getValue() << "\n"; + // } + + state = traj_states_ic.back(); + cout << state.pose->getValue() << endl; + cout << state.velocity->getValue().transpose() << endl; + cout << state.acceleration->getValue().transpose() << endl; + return 0; + +} From fe4aa47ee731567a51b48dbefe805809023bab56 Mon Sep 17 00:00:00 2001 From: TangTim Date: Thu, 19 Jul 2018 21:12:22 -0500 Subject: [PATCH 18/31] Added CA extrapolation functions (need checl) --- .../transform/ConstAccTransformEvaluator.hpp | 120 +++++++++++++++ .../transform/ConstAccTransformEvaluator.cpp | 142 ++++++++++++++++++ src/trajectory_ca/SteamCATrajInterface.cpp | 5 +- 3 files changed, 266 insertions(+), 1 deletion(-) create mode 100644 include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp create mode 100644 src/evaluator/blockauto/transform/ConstAccTransformEvaluator.cpp diff --git a/include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp b/include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp new file mode 100644 index 0000000..d61644e --- /dev/null +++ b/include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp @@ -0,0 +1,120 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file ConstAccTransformEvaluator.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CONSTANT_ACC_TRANSFORM_EVALUATOR_HPP +#define STEAM_CONSTANT_ACC_TRANSFORM_EVALUATOR_HPP + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Simple transform evaluator for a constant velocity model +////////////////////////////////////////////////////////////////////////////////////////////// +class ConstAccTransformEvaluator : public TransformEvaluator +{ + public: + + /// Convenience typedefs + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + ConstAccTransformEvaluator(const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Time& time); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Pseudo constructor - return a shared pointer to a new instance + ////////////////////////////////////////////////////////////////////////////////////////////// + static Ptr MakeShared(const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Time& time); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Adds references (shared pointers) to active state variables to the map output + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual void getActiveStateVariables( + std::map* outStates) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the transformation matrix + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual lgmath::se3::Transformation evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the transformation matrix tree + /// + /// ** Note that the returned pointer belongs to the memory pool EvalTreeNode::pool, + /// and should be given back to the pool, rather than being deleted. + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual EvalTreeNode* evaluateTree() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the Jacobian tree + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual void appendBlockAutomaticJacobians(const Eigen::MatrixXd& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Implementation for Block Automatic Differentiation + ////////////////////////////////////////////////////////////////////////////////////////////// + template + void appendJacobiansImpl(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Velocity state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::Ptr velocity_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Acceleration state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::Ptr acceleration_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Duration + ////////////////////////////////////////////////////////////////////////////////////////////// + Time time_; +}; + +} // se3 +} // steam + +#endif // STEAM_CONSTANT_ACC_TRANSFORM_EVALUATOR_HPP diff --git a/src/evaluator/blockauto/transform/ConstAccTransformEvaluator.cpp b/src/evaluator/blockauto/transform/ConstAccTransformEvaluator.cpp new file mode 100644 index 0000000..d461733 --- /dev/null +++ b/src/evaluator/blockauto/transform/ConstAccTransformEvaluator.cpp @@ -0,0 +1,142 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file ConstAccTransformEvaluator.cpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +ConstAccTransformEvaluator::ConstAccTransformEvaluator( + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Time& time) : velocity_(velocity), acceleration_(acceleration), + time_(time) { + + if(velocity->getPerturbDim() != 6) { + throw std::invalid_argument("[ConstVelTransformEval] velocity was not 6D."); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Pseudo constructor - return a shared pointer to a new instance +////////////////////////////////////////////////////////////////////////////////////////////// +ConstAccTransformEvaluator::Ptr ConstAccTransformEvaluator::MakeShared( + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Time& time) { + return ConstAccTransformEvaluator::Ptr(new ConstAccTransformEvaluator(velocity, + acceleration, time)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool ConstAccTransformEvaluator::isActive() const { + return !velocity_->isLocked(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Adds references (shared pointers) to active state variables to the map output +////////////////////////////////////////////////////////////////////////////////////////////// +void ConstAccTransformEvaluator::getActiveStateVariables( + std::map* outStates) const { + if (this->isActive()) { + (*outStates)[velocity_->getKey().getID()] = velocity_; + (*outStates)[acceleration_->getKey().getID()] = acceleration_; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the transformation matrix +////////////////////////////////////////////////////////////////////////////////////////////// +lgmath::se3::Transformation ConstAccTransformEvaluator::evaluate() const { + Eigen::Matrix xi = time_.seconds() * velocity_->getValue() + + 0.5*time_.seconds()*time_.seconds()*acceleration_->getValue(); + return lgmath::se3::Transformation(xi); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the transformation matrix tree +////////////////////////////////////////////////////////////////////////////////////////////// +EvalTreeNode* ConstAccTransformEvaluator::evaluateTree() const { + + // Make new leaf node -- note we get memory from the pool + EvalTreeNode* result = EvalTreeNode::pool.getObj(); + Eigen::Matrix xi = time_.seconds() * velocity_->getValue() + + 0.5*time_.seconds()*time_.seconds()*acceleration_->getValue(); + result->setValue(lgmath::se3::Transformation(xi)); + return result; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Implementation for Block Automatic Differentiation +////////////////////////////////////////////////////////////////////////////////////////////// +template +void ConstAccTransformEvaluator::appendJacobiansImpl( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + + if (!velocity_->isLocked()) { + + // Make jacobian + Eigen::Matrix xi = time_.seconds() * velocity_->getValue() + + 0.5*time_.seconds()*time_.seconds()*acceleration_->getValue(); + + // TODO: check this is correct + Eigen::Matrix jac_vel = time_.seconds() * lgmath::se3::vec2jac(xi); + Eigen::Matrix jac_acc = 0.5*time_.seconds()*time_.seconds()*lgmath::se3::vec2jac(xi); + + // Add Jacobian + outJacobians->push_back(Jacobian(velocity_->getKey(), lhs*jac_vel)); + outJacobians->push_back(Jacobian(acceleration_->getKey(), lhs*jac_acc)); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::MatrixXd& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +} // se3 +} // steam diff --git a/src/trajectory_ca/SteamCATrajInterface.cpp b/src/trajectory_ca/SteamCATrajInterface.cpp index 754854c..a454659 100644 --- a/src/trajectory_ca/SteamCATrajInterface.cpp +++ b/src/trajectory_ca/SteamCATrajInterface.cpp @@ -14,6 +14,7 @@ #include #include +#include namespace steam { namespace se3 { @@ -97,7 +98,9 @@ TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam --it1; // should be safe, as we checked that the map was not empty.. const SteamCATrajVar::Ptr& endKnot = it1->second; TransformEvaluator::Ptr T_t_k = - ConstVelTransformEvaluator::MakeShared(endKnot->getVelocity(), time - endKnot->getTime()); + ConstAccTransformEvaluator::MakeShared(endKnot->getVelocity(), + endKnot->getAcceleration(), + time - endKnot->getTime()); return compose(T_t_k, endKnot->getPose()); } else { throw std::runtime_error("Requested trajectory evaluator at an invalid time."); From 97278271788d2277f9372b7a0555d27b3f85c1e2 Mon Sep 17 00:00:00 2001 From: TangTim Date: Fri, 20 Jul 2018 00:38:47 -0500 Subject: [PATCH 19/31] fixed an issue where calculating interpolation constants require multiplication between very large numbers --- .../SteamCATrajPoseInterpEval.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp b/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp index 9d01fdd..7ba31b1 100644 --- a/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp +++ b/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp @@ -23,11 +23,18 @@ SteamCATrajPoseInterpEval::SteamCATrajPoseInterpEval(const Time& time, double t1 = knot1->getTime().seconds(); double t2 = knot2->getTime().seconds(); double tau = time.seconds(); + + // Cheat by calculating deltas wrt t1, so we can avoid super large values + tau = tau-t1; + t2 = t2-t1; + t1 = 0; double T = (knot2->getTime() - knot1->getTime()).seconds(); double delta_tau = (time - knot1->getTime()).seconds(); double delta_kappa = (knot2->getTime()-time).seconds(); + // std::cout << t1 << " " << t2 << " " << tau << std::endl; + double T2 = T*T; double T3 = T2*T; double T4 = T3*T; @@ -46,6 +53,9 @@ SteamCATrajPoseInterpEval::SteamCATrajPoseInterpEval(const Time& time, // Calculate 'lambda' interpolation values lambda12_ = delta_tau*delta_kappa3/T4*(t2 - 4*t1 + 3*tau); lambda13_ = delta_tau2*delta_kappa3/(2*T3); + + // std::cout << "omega11_" << omega11_ << std::endl; + // std::cout << "ratio: " << t1*t1 - 5*t1*t2 + 3*t1*tau + 10*t2*t2 - 15*t2*tau + 6*tau*tau << std::endl; } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -111,6 +121,15 @@ lgmath::se3::Transformation SteamCATrajPoseInterpEval::evaluate() const { lgmath::se3::Transformation T_i1(xi_i1); // Return `global' interpolated transform + // std::cout << "Interpolated xi_i1: " << std::endl; + // std::cout << xi_i1.transpose() << std::endl; + + // std::cout << "part1: " << lambda12_*knot1_->getVelocity()->getValue().transpose() << std::endl; + // std::cout << "part2: " << lambda13_*knot1_->getAcceleration()->getValue().transpose() << std::endl; + // std::cout << "part3: " << omega11_*xi_21.transpose() << std::endl; + // std::cout << "part4: " << (omega12_*J_21_inv*knot2_->getVelocity()->getValue()).transpose() << std::endl; + // std::cout << "part5: " << (omega13_*(-0.5*varpicurl2*knot2_->getVelocity()->getValue())).transpose() << std::endl; + // std::cout << "part6: " << (omega13_*J_21_inv*knot2_->getAcceleration()->getValue()).transpose() << std::endl; return T_i1*knot1_->getPose()->evaluate(); } From 454941d0d1c06b84e62cba8137694ba5230cf2cb Mon Sep 17 00:00:00 2001 From: TangTim Date: Wed, 1 Aug 2018 13:34:59 -0400 Subject: [PATCH 20/31] - use pre-computed terms in CAPrior error Jacobian - use CA interpolation when the time is before the startTime --- src/solver/GaussNewtonSolverBase.cpp | 4 ++++ src/trajectory_ca/SteamCATrajInterface.cpp | 15 ++++++++++----- src/trajectory_ca/SteamCATrajPoseInterpEval.cpp | 1 + src/trajectory_ca/SteamCATrajPriorFactor.cpp | 8 ++++++-- 4 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/solver/GaussNewtonSolverBase.cpp b/src/solver/GaussNewtonSolverBase.cpp index 8bf9e82..b093e52 100644 --- a/src/solver/GaussNewtonSolverBase.cpp +++ b/src/solver/GaussNewtonSolverBase.cpp @@ -153,6 +153,10 @@ void GaussNewtonSolverBase::factorizeHessian(const Eigen::SparseMatrix& // Check if the factorization succeeded if (hessianSolver_.info() != Eigen::Success) { + + std::cout << "Approximate Hessian is: " << std::endl; + std::cout << approximateHessian << std::endl; + throw decomp_failure("During steam solve, Eigen LLT decomposition failed. " "It is possible that the matrix was ill-conditioned, in which case " "adding a prior may help. On the other hand, it is also possible that " diff --git a/src/trajectory_ca/SteamCATrajInterface.cpp b/src/trajectory_ca/SteamCATrajInterface.cpp index a454659..1839880 100644 --- a/src/trajectory_ca/SteamCATrajInterface.cpp +++ b/src/trajectory_ca/SteamCATrajInterface.cpp @@ -93,7 +93,7 @@ TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam // Check if time is passed the last entry if (it1 == knotMap_.end()) { - // If we allow extrapolation, return constant-velocity interpolated entry + // If we allow extrapolation, return constant-acceleration interpolated entry if (allowExtrapolation_) { --it1; // should be safe, as we checked that the map was not empty.. const SteamCATrajVar::Ptr& endKnot = it1->second; @@ -117,12 +117,13 @@ TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam // Check if we requested before first time if (it1 == knotMap_.begin()) { - // If we allow extrapolation, return constant-velocity interpolated entry + // If we allow extrapolation, return constant-acceleration interpolated entry if (allowExtrapolation_) { const SteamCATrajVar::Ptr& startKnot = it1->second; TransformEvaluator::Ptr T_t_k = - ConstVelTransformEvaluator::MakeShared(startKnot->getVelocity(), - time - startKnot->getTime()); + ConstAccTransformEvaluator::MakeShared(startKnot->getVelocity(), + startKnot->getAcceleration(), + time - startKnot->getTime()); return compose(T_t_k, startKnot->getPose()); } else { throw std::runtime_error("Requested trajectory evaluator at an invalid time."); @@ -169,6 +170,7 @@ void SteamCATrajInterface::addPosePrior(const steam::Time& time, // Set up loss function, noise model, and error function steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + // steam::GemanMcClureLossFunc::Ptr sharedLossFunc(new steam::GemanMcClureLossFunc(1)); steam::BaseNoiseModel<6>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<6>(cov)); steam::TransformErrorEval::Ptr errorfunc(new steam::TransformErrorEval(pose, knotRef->getPose())); @@ -252,7 +254,8 @@ void SteamCATrajInterface::appendPriorCostTerms( // Check if any of the variables are unlocked if(knot1->getPose()->isActive() || !knot1->getVelocity()->isLocked() || - knot2->getPose()->isActive() || !knot2->getVelocity()->isLocked() ) { + knot2->getPose()->isActive() || !knot2->getVelocity()->isLocked() || + !knot2->getAcceleration()->isLocked() || !knot2->getAcceleration()->isLocked()) { // Generate 18 x 18 information matrix for GP prior factor Eigen::Matrix Qi_inv; @@ -269,6 +272,8 @@ void SteamCATrajInterface::appendPriorCostTerms( Qi_inv.block<6,6>(12,0) = Qi_inv.block<6,6>(0,12) = 60.0 * one_over_dt3 * Qc_inv_; Qi_inv.block<6,6>(12,6) = Qi_inv.block<6,6>(6,12) = -36.0 * one_over_dt2 * Qc_inv_; + // std::cout << Qi_inv.block<6,6>(12,0) << std::endl << std::endl; + // std::cout << Qi_inv.block<6,6>(0,12) << std::endl; steam::BaseNoiseModelX::Ptr sharedGPNoiseModel( new steam::StaticNoiseModelX(Qi_inv, steam::INFORMATION)); diff --git a/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp b/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp index 7ba31b1..08650a6 100644 --- a/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp +++ b/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp @@ -54,6 +54,7 @@ SteamCATrajPoseInterpEval::SteamCATrajPoseInterpEval(const Time& time, lambda12_ = delta_tau*delta_kappa3/T4*(t2 - 4*t1 + 3*tau); lambda13_ = delta_tau2*delta_kappa3/(2*T3); + // std::cout << "CA interpolation!" << std::endl; // std::cout << "omega11_" << omega11_ << std::endl; // std::cout << "ratio: " << t1*t1 - 5*t1*t2 + 3*t1*tau + 10*t2*t2 - 15*t2*tau + 6*tau*tau << std::endl; } diff --git a/src/trajectory_ca/SteamCATrajPriorFactor.cpp b/src/trajectory_ca/SteamCATrajPriorFactor.cpp index 69d69e5..ff1dcf3 100644 --- a/src/trajectory_ca/SteamCATrajPriorFactor.cpp +++ b/src/trajectory_ca/SteamCATrajPriorFactor.cpp @@ -82,7 +82,7 @@ Eigen::VectorXd SteamCATrajPriorFactor::evaluate(const Eigen::MatrixXd& lhs, // Construct jacobian Eigen::Matrix jacobian; jacobian.topRows<6>() = -Jinv_12; - jacobian.block<6,6>(6,0) = -0.5*lgmath::se3::curlyhat(knot2_->getVelocity()->getValue())*Jinv_12; + jacobian.block<6,6>(6,0) = -0.5*v2curl*Jinv_12; jacobian.bottomRows<6>() = -0.25*v2curl*v2curl*Jinv_12 - 0.5*a2curl*Jinv_12; // Get Jacobians @@ -98,7 +98,7 @@ Eigen::VectorXd SteamCATrajPriorFactor::evaluate(const Eigen::MatrixXd& lhs, // Construct jacobian Eigen::Matrix jacobian; jacobian.topRows<6>() = J_21_inv; - jacobian.block<6,6>(6,0) = 0.5*lgmath::se3::curlyhat(knot2_->getVelocity()->getValue())*J_21_inv; + jacobian.block<6,6>(6,0) = 0.5*v2curl*J_21_inv; jacobian.bottomRows<6>() = 0.25*v2curl*v2curl*J_21_inv + 0.5*a2curl*J_21_inv; // Get Jacobians @@ -155,6 +155,8 @@ Eigen::VectorXd SteamCATrajPriorFactor::evaluate(const Eigen::MatrixXd& lhs, jacobian.block<6,6>(6,0) = -deltaTime*Eigen::Matrix::Identity(); jacobian.bottomRows<6>() = -Eigen::Matrix::Identity(); jacref.jac = lhs * jacobian; + + // std::cout << jacobian << std::endl; } // Knot 2 acceleration @@ -182,6 +184,8 @@ Eigen::VectorXd SteamCATrajPriorFactor::evaluate(const Eigen::MatrixXd& lhs, error.head<6>() = xi_21 - deltaTime*knot1_->getVelocity()->getValue() - 0.5*dt2*knot1_->getAcceleration()->getValue(); error.segment<6>(6) = J_21_inv*knot2_->getVelocity()->getValue() - knot1_->getVelocity()->getValue() - deltaTime*knot1_->getAcceleration()->getValue(); error.tail<6>() = -0.5*w*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue() - knot1_->getAcceleration()->getValue(); + + // std::cout << error.transpose() << std::endl; return error; } From 07e67870955ed14f8a7f2c3f7f0511b94ce8899e Mon Sep 17 00:00:00 2001 From: TangTim Date: Wed, 8 Aug 2018 19:07:51 -0400 Subject: [PATCH 21/31] - getting ready to add CVPriorFactor in CA trajectory --- .../trajectory/SteamCustomPriorFactor.hpp | 67 +++++++++++++++++++ .../trajectory_ca/SteamCATrajInterface.hpp | 5 ++ src/trajectory_ca/SteamCATrajInterface.cpp | 48 +++++++++++-- src/trajectory_ca/SteamCATrajPriorFactor.cpp | 1 - 4 files changed, 116 insertions(+), 5 deletions(-) create mode 100644 include/steam/trajectory/SteamCustomPriorFactor.hpp diff --git a/include/steam/trajectory/SteamCustomPriorFactor.hpp b/include/steam/trajectory/SteamCustomPriorFactor.hpp new file mode 100644 index 0000000..c63f697 --- /dev/null +++ b/include/steam/trajectory/SteamCustomPriorFactor.hpp @@ -0,0 +1,67 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamTrajPriorFactor.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CUSTOM_PRIOR_FACTOR_HPP +#define STEAM_CUSTOM_PRIOR_FACTOR_HPP + +#include + +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Gaussian-process prior evaluator +////////////////////////////////////////////////////////////////////////////////////////////// +class SteamCustomPriorFactor : public ErrorEvaluatorX +{ + public: + + /// Shared pointer typedefs for readability + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCustomPriorFactor(const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the GP prior factor + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::VectorXd evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the GP prior factor and Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::VectorXd evaluate(const Eigen::MatrixXd& lhs, std::vector >* jacs) const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief First (earlier) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamTrajVar::ConstPtr knot1_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Second (later) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamTrajVar::ConstPtr knot2_; +}; + +} // se3 +} // steam + +#endif // STEAM_CUSTOM_PRIOR_FACTOR_HPP + diff --git a/include/steam/trajectory_ca/SteamCATrajInterface.hpp b/include/steam/trajectory_ca/SteamCATrajInterface.hpp index 4edd867..1f3da99 100644 --- a/include/steam/trajectory_ca/SteamCATrajInterface.hpp +++ b/include/steam/trajectory_ca/SteamCATrajInterface.hpp @@ -110,6 +110,11 @@ class SteamCATrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// steam::WeightedLeastSqCostTerm<6,6>::Ptr velocityPriorFactor_; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Acceleration prior + ////////////////////////////////////////////////////////////////////////////////////////////// + steam::WeightedLeastSqCostTerm<6,6>::Ptr accelerationPriorFactor_; + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Ordered map of knots ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/src/trajectory_ca/SteamCATrajInterface.cpp b/src/trajectory_ca/SteamCATrajInterface.cpp index 1839880..76425f5 100644 --- a/src/trajectory_ca/SteamCATrajInterface.cpp +++ b/src/trajectory_ca/SteamCATrajInterface.cpp @@ -170,7 +170,6 @@ void SteamCATrajInterface::addPosePrior(const steam::Time& time, // Set up loss function, noise model, and error function steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); - // steam::GemanMcClureLossFunc::Ptr sharedLossFunc(new steam::GemanMcClureLossFunc(1)); steam::BaseNoiseModel<6>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<6>(cov)); steam::TransformErrorEval::Ptr errorfunc(new steam::TransformErrorEval(pose, knotRef->getPose())); @@ -203,7 +202,7 @@ void SteamCATrajInterface::addVelocityPrior(const steam::Time& time, // Check that the pose is not locked if(knotRef->getVelocity()->isLocked()) { - throw std::runtime_error("[GpTrajectory][addVelocityPrior] tried to add prior to locked pose."); + throw std::runtime_error("[GpTrajectory][addVelocityPrior] tried to add prior to locked velocity."); } // Set up loss function, noise model, and error function @@ -216,6 +215,44 @@ void SteamCATrajInterface::addVelocityPrior(const steam::Time& time, new steam::WeightedLeastSqCostTerm<6,6>(errorfunc, sharedNoiseModel, sharedLossFunc)); } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Add a unary acceleration prior factor at a knot time. Note that only a single acceleration +/// prior should exist on a trajectory, adding a second will overwrite the first. +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajInterface::addAccelerationPrior(const steam::Time& time, + const Eigen::Matrix& acceleration, + const Eigen::Matrix& cov) { + + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][addVelocityPrior] map was empty."); + } + + // Try to find knot at same time + std::map::const_iterator it = knotMap_.find(time.nanosecs()); + if (it == knotMap_.end()) { + throw std::runtime_error("[GpTrajectory][addVelocityPrior] no knot at provided time."); + } + + // Get reference + const SteamCATrajVar::Ptr& knotRef = it->second; + + // Check that the pose is not locked + if(knotRef->getAcceleration()->isLocked()) { + throw std::runtime_error("[GpTrajectory][addAccelerationPrior] tried to add prior to locked acceleration."); + } + + // Set up loss function, noise model, and error function + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + steam::BaseNoiseModel<6>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<6>(cov)); + steam::VectorSpaceErrorEval<6,6>::Ptr errorfunc(new steam::VectorSpaceErrorEval<6,6>(acceleration, knotRef->getAcceleration())); + + // Create cost term + accelerationPriorFactor_ = steam::WeightedLeastSqCostTerm<6,6>::Ptr( + new steam::WeightedLeastSqCostTerm<6,6>(errorfunc, sharedNoiseModel, sharedLossFunc)); +} + + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get cost terms associated with the prior for unlocked parts of the trajectory ////////////////////////////////////////////////////////////////////////////////////////////// @@ -227,16 +264,19 @@ void SteamCATrajInterface::appendPriorCostTerms( return; } - // Check for pose or velocity priors + // Check for pose, velocity priors, and acceleration priors if (posePriorFactor_) { costTerms->add(posePriorFactor_); } if (velocityPriorFactor_) { costTerms->add(velocityPriorFactor_); } - + if (accelerationPriorFactor_) { + costTerms->add(accelerationPriorFactor_); + } // All prior factors will use an L2 loss function steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + // steam::GemanMcClureLossFunc::Ptr sharedLossFunc(new steam::GemanMcClureLossFunc(1)); // Initialize first iterator std::map::const_iterator it1 = knotMap_.begin(); diff --git a/src/trajectory_ca/SteamCATrajPriorFactor.cpp b/src/trajectory_ca/SteamCATrajPriorFactor.cpp index ff1dcf3..0f58b49 100644 --- a/src/trajectory_ca/SteamCATrajPriorFactor.cpp +++ b/src/trajectory_ca/SteamCATrajPriorFactor.cpp @@ -185,7 +185,6 @@ Eigen::VectorXd SteamCATrajPriorFactor::evaluate(const Eigen::MatrixXd& lhs, error.segment<6>(6) = J_21_inv*knot2_->getVelocity()->getValue() - knot1_->getVelocity()->getValue() - deltaTime*knot1_->getAcceleration()->getValue(); error.tail<6>() = -0.5*w*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue() - knot1_->getAcceleration()->getValue(); - // std::cout << error.transpose() << std::endl; return error; } From 88ce8412a03e857d7e2d7eb46f5e1ecdd64306c9 Mon Sep 17 00:00:00 2001 From: Jeremy Wong Date: Thu, 1 Aug 2019 10:50:59 -0400 Subject: [PATCH 22/31] Add velocity interpolation to WNOJ prior --- .../trajectory_ca/SteamCATrajInterface.hpp | 5 + src/trajectory_ca/SteamCATrajInterface.cpp | 129 ++++++++++++++++++ 2 files changed, 134 insertions(+) diff --git a/include/steam/trajectory_ca/SteamCATrajInterface.hpp b/include/steam/trajectory_ca/SteamCATrajInterface.hpp index 1f3da99..ff62dbb 100644 --- a/include/steam/trajectory_ca/SteamCATrajInterface.hpp +++ b/include/steam/trajectory_ca/SteamCATrajInterface.hpp @@ -56,6 +56,11 @@ class SteamCATrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// TransformEvaluator::ConstPtr getInterpPoseEval(const steam::Time& time) const; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get velocity evaluator + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::VectorXd getVelocity(const steam::Time& time); + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior /// should exist on a trajectory, adding a second will overwrite the first. diff --git a/src/trajectory_ca/SteamCATrajInterface.cpp b/src/trajectory_ca/SteamCATrajInterface.cpp index 76425f5..051dbc9 100644 --- a/src/trajectory_ca/SteamCATrajInterface.cpp +++ b/src/trajectory_ca/SteamCATrajInterface.cpp @@ -141,6 +141,135 @@ TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam return SteamCATrajPoseInterpEval::MakeShared(time, it1->second, it2->second); } +Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); + } + + // Get iterator to first element with time equal to or greater than 'time' + std::map::const_iterator it1 + = knotMap_.lower_bound(time.nanosecs()); + + // Check if time is passed the last entry + if (it1 == knotMap_.end()) { + + // If we allow extrapolation, return constant-velocity interpolated entry + if (allowExtrapolation_) { + --it1; // should be safe, as we checked that the map was not empty.. + const SteamCATrajVar::Ptr& endKnot = it1->second; + return endKnot->getVelocity()->getValue(); + } else { + throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + } + } + + // Check if we requested time exactly + if (it1->second->getTime() == time) { + const SteamCATrajVar::Ptr& knot = it1->second; + // return state variable exactly (no interp) + return knot->getVelocity()->getValue(); + } + + // Check if we requested before first time + if (it1 == knotMap_.begin()) { + // If we allow extrapolation, return constant-velocity interpolated entry + if (allowExtrapolation_) { + const SteamCATrajVar::Ptr& startKnot = it1->second; + return startKnot->getVelocity()->getValue(); + } else { + throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + } + } + + // Get iterators bounding the time interval + std::map::const_iterator it2 = it1; --it1; + if (time <= it1->second->getTime() || time >= it2->second->getTime()) { + throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " + "should not trigger... report to a STEAM contributor."); + } + + // OK, we actually need to interpolate. + // Follow a similar setup to SteamCATrajPoseInterpEval + + // Convenience defs + auto &knot1 = it1->second; + auto &knot2 = it2->second; + + double t1 = knot1->getTime().seconds(); + double t2 = knot2->getTime().seconds(); + double tau = time.seconds(); + + // Cheat by calculating deltas wrt t1, so we can avoid super large values + tau = tau-t1; + t2 = t2-t1; + t1 = 0; + + double T = (knot2->getTime() - knot1->getTime()).seconds(); + double delta_tau = (time - knot1->getTime()).seconds(); + double delta_kappa = (knot2->getTime()-time).seconds(); + + // std::cout << t1 << " " << t2 << " " << tau << std::endl; + + double T2 = T*T; + double T3 = T2*T; + double T4 = T3*T; + double T5 = T4*T; + + double delta_tau2 = delta_tau*delta_tau; + double delta_tau3 = delta_tau2*delta_tau; + double delta_tau4 = delta_tau3*delta_tau; + double delta_kappa2 = delta_kappa*delta_kappa; + double delta_kappa3 = delta_kappa2*delta_kappa; + + // Calculate 'omega' interpolation values + double omega11 = delta_tau3/T5*(t1*t1 - 5*t1*t2 + 3*t1*tau + 10*t2*t2 - 15*t2*tau + 6*tau*tau); + double omega12 = delta_tau3*delta_kappa/T4*(t1 - 4*t2 + 3*tau); + double omega13 = delta_tau3*delta_kappa2/(2*T3); + + double omega21 = 30*delta_tau2/T5*(6*delta_kappa2-4*delta_tau*T+8*delta_tau*delta_kappa-6*T*delta_kappa+3*delta_tau2+T2); + double omega22 = -delta_tau2/T4*(90*delta_kappa2-64*delta_tau*T+120*delta_tau*delta_kappa-96*T*delta_kappa+45*delta_tau2+18*T2); + double omega23 = delta_tau2/(2*T3)*(30*delta_kappa2-24*delta_tau*T+40*delta_tau*delta_kappa-36*T*delta_kappa+15*delta_tau2+9*T2); + + // Calculate 'lambda' interpolation values + double lambda12 = delta_tau*delta_kappa3/T4*(t2 - 4*t1 + 3*tau); + double lambda13 = delta_tau2*delta_kappa3/(2*T3); + + double lambda22 = -(90*delta_tau2*delta_kappa2-56*delta_tau3*T+45*delta_tau4-T4+120*delta_tau3*delta_kappa+12*delta_tau2*T2-84*delta_tau2*T*delta_kappa)/T4; + double lambda23 = -delta_tau/(2*T3)*(3*delta_tau*T2-16*delta_tau2*T+15*delta_tau3-2*T3+30*delta_tau*delta_kappa2+40*delta_tau2*delta_kappa-24*delta_tau*T*delta_kappa); + + // Get relative matrix info + lgmath::se3::Transformation T_21 = knot2->getPose()->evaluate()/knot1->getPose()->evaluate(); + + // Get se3 algebra of relative matrix + Eigen::Matrix xi_21 = T_21.vec(); + + // Calculate the 6x6 associated Jacobian + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + + // Intermediate variable + Eigen::Matrix varpicurl2 = lgmath::se3::curlyhat(J_21_inv*knot2->getVelocity()->getValue()); + + // Calculate interpolated relative se3 algebra + Eigen::Matrix xi_i1 = lambda12*knot1->getVelocity()->getValue() + + lambda13*knot1->getAcceleration()->getValue() + + omega11*xi_21 + + omega12*J_21_inv*knot2->getVelocity()->getValue()+ + omega13*(-0.5*varpicurl2*knot2->getVelocity()->getValue() + J_21_inv*knot2->getAcceleration()->getValue()); + + // Calculate the 6x6 associated Jacobian + Eigen::Matrix J_t1 = lgmath::se3::vec2jac(xi_i1); + + // Calculate interpolated relative se3 algebra + Eigen::VectorXd xi_it = J_t1*(lambda22*knot1->getVelocity()->getValue() + + lambda23*knot1->getAcceleration()->getValue() + + omega21*xi_21 + + omega22*J_21_inv*knot2->getVelocity()->getValue() + + omega23*(-0.5*varpicurl2*knot2->getVelocity()->getValue() + J_21_inv*knot2->getAcceleration()->getValue())); + + return xi_it; + } + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior /// should exist on a trajectory, adding a second will overwrite the first. From 66f6bb62edc1cf2b544ef8a5917cd062ea86233f Mon Sep 17 00:00:00 2001 From: Jeremy Wong Date: Tue, 7 Apr 2020 16:11:53 -0400 Subject: [PATCH 23/31] SteamCATrajInterface derived class of SteamTrajInterface --- .../steam/trajectory/SteamTrajInterface.hpp | 18 +++--- .../trajectory_ca/SteamCATrajInterface.hpp | 39 ++----------- .../steam/trajectory_ca/SteamCATrajVar.hpp | 34 +----------- samples/SimpleBAandCATrajPrior.cpp | 9 ++- src/trajectory/SteamTrajInterface.cpp | 6 ++ src/trajectory_ca/SteamCATrajInterface.cpp | 55 +++++++------------ src/trajectory_ca/SteamCATrajVar.cpp | 23 +------- 7 files changed, 53 insertions(+), 131 deletions(-) diff --git a/include/steam/trajectory/SteamTrajInterface.hpp b/include/steam/trajectory/SteamTrajInterface.hpp index 67bfacd..7505615 100644 --- a/include/steam/trajectory/SteamTrajInterface.hpp +++ b/include/steam/trajectory/SteamTrajInterface.hpp @@ -50,45 +50,49 @@ class SteamTrajInterface void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, const VectorSpaceStateVar::Ptr& velocity); + virtual void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration); + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get transform evaluator ////////////////////////////////////////////////////////////////////////////////////////////// - TransformEvaluator::ConstPtr getInterpPoseEval(const steam::Time& time) const; + virtual TransformEvaluator::ConstPtr getInterpPoseEval(const steam::Time& time) const; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get velocity evaluator ////////////////////////////////////////////////////////////////////////////////////////////// - Eigen::VectorXd getVelocity(const steam::Time& time); + virtual Eigen::VectorXd getVelocity(const steam::Time& time); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior /// should exist on a trajectory, adding a second will overwrite the first. ////////////////////////////////////////////////////////////////////////////////////////////// - void addPosePrior(const steam::Time& time, const lgmath::se3::Transformation& pose, + virtual void addPosePrior(const steam::Time& time, const lgmath::se3::Transformation& pose, const Eigen::Matrix& cov); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary velocity prior factor at a knot time. Note that only a single velocity /// prior should exist on a trajectory, adding a second will overwrite the first. ////////////////////////////////////////////////////////////////////////////////////////////// - void addVelocityPrior(const steam::Time& time, const Eigen::Matrix& velocity, + virtual void addVelocityPrior(const steam::Time& time, const Eigen::Matrix& velocity, const Eigen::Matrix& cov); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get binary cost terms associated with the prior for active parts of the trajectory ////////////////////////////////////////////////////////////////////////////////////////////// - void appendPriorCostTerms(const ParallelizedCostTermCollection::Ptr& costTerms) const; + virtual void appendPriorCostTerms(const ParallelizedCostTermCollection::Ptr& costTerms) const; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get active state variables in the trajectory ////////////////////////////////////////////////////////////////////////////////////////////// - void getActiveStateVariables( + virtual void getActiveStateVariables( std::map* outStates) const; double getPosePriorCost(); double getVelocityPriorCost(); - private: + protected: ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Ordered map of knots diff --git a/include/steam/trajectory_ca/SteamCATrajInterface.hpp b/include/steam/trajectory_ca/SteamCATrajInterface.hpp index ff62dbb..f572e32 100644 --- a/include/steam/trajectory_ca/SteamCATrajInterface.hpp +++ b/include/steam/trajectory_ca/SteamCATrajInterface.hpp @@ -12,6 +12,7 @@ #include #include +#include #include #include @@ -23,21 +24,11 @@ namespace se3 { /// \brief The trajectory class wraps a set of state variables to provide an interface /// that allows for continuous-time pose interpolation. ////////////////////////////////////////////////////////////////////////////////////////////// -class SteamCATrajInterface +class SteamCATrajInterface : public SteamTrajInterface { public: - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Constructor - /// Note that the weighting matrix, Qc, should be provided if prior factors are needed - /// for estimation. Without Qc the interpolation methods can be used safely. - ////////////////////////////////////////////////////////////////////////////////////////////// - SteamCATrajInterface(bool allowExtrapolation = false); - - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Constructor - ////////////////////////////////////////////////////////////////////////////////////////////// - SteamCATrajInterface(const Eigen::Matrix& Qc_inv, bool allowExtrapolation = false); + using SteamTrajInterface::SteamTrajInterface; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot @@ -90,31 +81,11 @@ class SteamCATrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get active state variables in the trajectory ////////////////////////////////////////////////////////////////////////////////////////////// - void getActiveStateVariables( - std::map* outStates) const; + // void getActiveStateVariables( + // std::map* outStates) const; private: - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Ordered map of knots - ////////////////////////////////////////////////////////////////////////////////////////////// - Eigen::Matrix Qc_inv_; - - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Allow for extrapolation - ////////////////////////////////////////////////////////////////////////////////////////////// - bool allowExtrapolation_; - - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Pose prior - ////////////////////////////////////////////////////////////////////////////////////////////// - steam::WeightedLeastSqCostTerm<6,6>::Ptr posePriorFactor_; - - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Velocity prior - ////////////////////////////////////////////////////////////////////////////////////////////// - steam::WeightedLeastSqCostTerm<6,6>::Ptr velocityPriorFactor_; - ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Acceleration prior ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/include/steam/trajectory_ca/SteamCATrajVar.hpp b/include/steam/trajectory_ca/SteamCATrajVar.hpp index 72162cb..add3ffa 100644 --- a/include/steam/trajectory_ca/SteamCATrajVar.hpp +++ b/include/steam/trajectory_ca/SteamCATrajVar.hpp @@ -13,6 +13,8 @@ #include #include +#include + namespace steam { namespace se3 { @@ -20,7 +22,7 @@ namespace se3 { /// \brief This class wraps a pose and velocity evaluator to act as a discrete-time trajectory /// state variable for continuous-time trajectory estimation. ////////////////////////////////////////////////////////////////////////////////////////////// -class SteamCATrajVar +class SteamCATrajVar : public SteamTrajVar { public: @@ -35,43 +37,13 @@ class SteamCATrajVar const VectorSpaceStateVar::Ptr& velocity, const VectorSpaceStateVar::Ptr& acceleration); - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get pose evaluator - ////////////////////////////////////////////////////////////////////////////////////////////// - const se3::TransformEvaluator::Ptr& getPose() const; - - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get velocity state variable - ////////////////////////////////////////////////////////////////////////////////////////////// - const VectorSpaceStateVar::Ptr& getVelocity() const; - ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get velocity state variable ////////////////////////////////////////////////////////////////////////////////////////////// const VectorSpaceStateVar::Ptr& getAcceleration() const; - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get timestamp - ////////////////////////////////////////////////////////////////////////////////////////////// - const steam::Time& getTime() const; - private: - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Timestamp of trajectory variable - ////////////////////////////////////////////////////////////////////////////////////////////// - steam::Time time_; - - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Pose evaluator - ////////////////////////////////////////////////////////////////////////////////////////////// - se3::TransformEvaluator::Ptr T_k0_; - - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Generalized 6D velocity state variable - ////////////////////////////////////////////////////////////////////////////////////////////// - VectorSpaceStateVar::Ptr velocity_; - ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Generalized 6D acceleration state variable ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/samples/SimpleBAandCATrajPrior.cpp b/samples/SimpleBAandCATrajPrior.cpp index ffcbf56..a73d108 100644 --- a/samples/SimpleBAandCATrajPrior.cpp +++ b/samples/SimpleBAandCATrajPrior.cpp @@ -129,7 +129,14 @@ int main(int argc, char** argv) { } // Setup Trajectory - steam::se3::SteamCATrajInterface traj(Qc_inv); + // steam::se3::SteamCATrajInterface ca_traj(Qc_inv); + // steam::se3::SteamTrajInterface *traj_ptr=&ca_traj; + // steam::se3::SteamTrajInterface traj=&traj_ptr; + + steam::se3::SteamCATrajInterface ca_traj(Qc_inv); + steam::se3::SteamTrajInterface &traj{ca_traj}; + + // steam::se3::SteamCATrajInterface traj(Qc_inv); for (unsigned int i = 0; i < traj_states_ic.size(); i++) { TrajStateVar& state = traj_states_ic.at(i); steam::se3::TransformStateEvaluator::Ptr temp = diff --git a/src/trajectory/SteamTrajInterface.cpp b/src/trajectory/SteamTrajInterface.cpp index 1db4b2c..94d7176 100644 --- a/src/trajectory/SteamTrajInterface.cpp +++ b/src/trajectory/SteamTrajInterface.cpp @@ -84,6 +84,12 @@ void SteamTrajInterface::add(const steam::Time& time, std::pair(time.nanosecs(), newEntry)); } +void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration) { + add(time, T_k0, velocity); +} + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get evaluator ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/src/trajectory_ca/SteamCATrajInterface.cpp b/src/trajectory_ca/SteamCATrajInterface.cpp index 051dbc9..59ce3ae 100644 --- a/src/trajectory_ca/SteamCATrajInterface.cpp +++ b/src/trajectory_ca/SteamCATrajInterface.cpp @@ -19,23 +19,6 @@ namespace steam { namespace se3 { -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Constructor -/// Note, without providing Qc, the trajectory can be used safely for interpolation, -/// but should not be used for estimation. -////////////////////////////////////////////////////////////////////////////////////////////// -SteamCATrajInterface::SteamCATrajInterface(bool allowExtrapolation) : - Qc_inv_(Eigen::Matrix::Identity()), allowExtrapolation_(allowExtrapolation) { -} - -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Constructor -////////////////////////////////////////////////////////////////////////////////////////////// -SteamCATrajInterface::SteamCATrajInterface(const Eigen::Matrix& Qc_inv, - bool allowExtrapolation) : - Qc_inv_(Qc_inv), allowExtrapolation_(allowExtrapolation) { -} - ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// @@ -456,25 +439,25 @@ void SteamCATrajInterface::appendPriorCostTerms( } } -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Get active state variables in the trajectory -////////////////////////////////////////////////////////////////////////////////////////////// -void SteamCATrajInterface::getActiveStateVariables( - std::map* outStates) const { - - // Iterate over trajectory - std::map::const_iterator it; - for (it = knotMap_.begin(); it != knotMap_.end(); ++it) { - - // Append active states in transform evaluator - it->second->getPose()->getActiveStateVariables(outStates); - - // Check if velocity is locked - if (!it->second->getVelocity()->isLocked()) { - (*outStates)[it->second->getVelocity()->getKey().getID()] = it->second->getVelocity(); - } - } -} +// ////////////////////////////////////////////////////////////////////////////////////////////// +// /// \brief Get active state variables in the trajectory +// ////////////////////////////////////////////////////////////////////////////////////////////// +// void SteamCATrajInterface::getActiveStateVariables( +// std::map* outStates) const { + +// // Iterate over trajectory +// std::map::const_iterator it; +// for (it = knotMap_.begin(); it != knotMap_.end(); ++it) { + +// // Append active states in transform evaluator +// it->second->getPose()->getActiveStateVariables(outStates); + +// // Check if velocity is locked +// if (!it->second->getVelocity()->isLocked()) { +// (*outStates)[it->second->getVelocity()->getKey().getID()] = it->second->getVelocity(); +// } +// } +// } } // se3 } // steam diff --git a/src/trajectory_ca/SteamCATrajVar.cpp b/src/trajectory_ca/SteamCATrajVar.cpp index 194eaf5..04bd632 100644 --- a/src/trajectory_ca/SteamCATrajVar.cpp +++ b/src/trajectory_ca/SteamCATrajVar.cpp @@ -18,7 +18,7 @@ SteamCATrajVar::SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, const VectorSpaceStateVar::Ptr& velocity, const VectorSpaceStateVar::Ptr& acceleration) - : time_(time), T_k0_(T_k0), velocity_(velocity), acceleration_(acceleration) { + : SteamTrajVar(time, T_k0, velocity), acceleration_(acceleration) { // Check velocity input if (velocity->getPerturbDim() != 6) { @@ -31,20 +31,6 @@ SteamCATrajVar::SteamCATrajVar(const steam::Time& time, } } -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Get pose evaluator -////////////////////////////////////////////////////////////////////////////////////////////// -const se3::TransformEvaluator::Ptr& SteamCATrajVar::getPose() const { - return T_k0_; -} - -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Get velocity state variable -////////////////////////////////////////////////////////////////////////////////////////////// -const VectorSpaceStateVar::Ptr& SteamCATrajVar::getVelocity() const { - return velocity_; -} - ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get acceleration state variable ////////////////////////////////////////////////////////////////////////////////////////////// @@ -52,12 +38,5 @@ const VectorSpaceStateVar::Ptr& SteamCATrajVar::getAcceleration() const { return acceleration_; } -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Get timestamp -////////////////////////////////////////////////////////////////////////////////////////////// -const steam::Time& SteamCATrajVar::getTime() const { - return time_; -} - } // se3 } // steam From fddc482cf289ef467fd8388c0cf8dc69a388b4de Mon Sep 17 00:00:00 2001 From: Jeremy Wong Date: Wed, 22 Apr 2020 22:44:37 -0400 Subject: [PATCH 24/31] exploit polymorphism of SteamTrajVar --- .../steam/trajectory/SteamTrajInterface.hpp | 4 +- include/steam/trajectory/SteamTrajVar.hpp | 5 + .../trajectory_ca/SteamCATrajInterface.hpp | 18 +-- .../SteamCATrajPoseInterpEval.hpp | 12 +- .../trajectory_ca/SteamCATrajPriorFactor.hpp | 8 +- samples/SimpleBAandCATrajPrior.cpp | 7 +- src/trajectory/SteamTrajVar.cpp | 5 + src/trajectory_ca/SteamCATrajInterface.cpp | 118 ++++-------------- .../SteamCATrajPoseInterpEval.cpp | 8 +- src/trajectory_ca/SteamCATrajPriorFactor.cpp | 4 +- 10 files changed, 55 insertions(+), 134 deletions(-) diff --git a/include/steam/trajectory/SteamTrajInterface.hpp b/include/steam/trajectory/SteamTrajInterface.hpp index 7505615..eccee76 100644 --- a/include/steam/trajectory/SteamTrajInterface.hpp +++ b/include/steam/trajectory/SteamTrajInterface.hpp @@ -68,14 +68,14 @@ class SteamTrajInterface /// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior /// should exist on a trajectory, adding a second will overwrite the first. ////////////////////////////////////////////////////////////////////////////////////////////// - virtual void addPosePrior(const steam::Time& time, const lgmath::se3::Transformation& pose, + void addPosePrior(const steam::Time& time, const lgmath::se3::Transformation& pose, const Eigen::Matrix& cov); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary velocity prior factor at a knot time. Note that only a single velocity /// prior should exist on a trajectory, adding a second will overwrite the first. ////////////////////////////////////////////////////////////////////////////////////////////// - virtual void addVelocityPrior(const steam::Time& time, const Eigen::Matrix& velocity, + void addVelocityPrior(const steam::Time& time, const Eigen::Matrix& velocity, const Eigen::Matrix& cov); ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/include/steam/trajectory/SteamTrajVar.hpp b/include/steam/trajectory/SteamTrajVar.hpp index dcb5cf7..2dfd603 100644 --- a/include/steam/trajectory/SteamTrajVar.hpp +++ b/include/steam/trajectory/SteamTrajVar.hpp @@ -44,6 +44,11 @@ class SteamTrajVar ////////////////////////////////////////////////////////////////////////////////////////////// const VectorSpaceStateVar::Ptr& getVelocity() const; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get acceleration state variable, to be overriden + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual const VectorSpaceStateVar::Ptr& getAcceleration() const; + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get timestamp ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/include/steam/trajectory_ca/SteamCATrajInterface.hpp b/include/steam/trajectory_ca/SteamCATrajInterface.hpp index f572e32..dbc3a16 100644 --- a/include/steam/trajectory_ca/SteamCATrajInterface.hpp +++ b/include/steam/trajectory_ca/SteamCATrajInterface.hpp @@ -33,7 +33,7 @@ class SteamCATrajInterface : public SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// - void add(const SteamCATrajVar::Ptr& knot); + // void add(const SteamCATrajVar::Ptr& knot); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot @@ -52,20 +52,6 @@ class SteamCATrajInterface : public SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// Eigen::VectorXd getVelocity(const steam::Time& time); - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior - /// should exist on a trajectory, adding a second will overwrite the first. - ////////////////////////////////////////////////////////////////////////////////////////////// - void addPosePrior(const steam::Time& time, const lgmath::se3::Transformation& pose, - const Eigen::Matrix& cov); - - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Add a unary velocity prior factor at a knot time. Note that only a single velocity - /// prior should exist on a trajectory, adding a second will overwrite the first. - ////////////////////////////////////////////////////////////////////////////////////////////// - void addVelocityPrior(const steam::Time& time, const Eigen::Matrix& velocity, - const Eigen::Matrix& cov); - ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary acceleration prior factor at a knot time. Note that only a single acceleration /// prior should exist on a trajectory, adding a second will overwrite the first. @@ -94,7 +80,7 @@ class SteamCATrajInterface : public SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Ordered map of knots ////////////////////////////////////////////////////////////////////////////////////////////// - std::map knotMap_; + // std::map knotMap_; }; diff --git a/include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp b/include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp index f5cc741..9165d7a 100644 --- a/include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp +++ b/include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp @@ -26,15 +26,15 @@ class SteamCATrajPoseInterpEval : public TransformEvaluator /// \brief Constructor ////////////////////////////////////////////////////////////////////////////////////////////// SteamCATrajPoseInterpEval(const Time& time, - const SteamCATrajVar::ConstPtr& knot1, - const SteamCATrajVar::ConstPtr& knot2); + const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Pseudo constructor - return a shared pointer to a new instance ////////////////////////////////////////////////////////////////////////////////////////////// static Ptr MakeShared(const Time& time, - const SteamCATrajVar::ConstPtr& knot1, - const SteamCATrajVar::ConstPtr& knot2); + const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Returns whether or not an evaluator contains unlocked state variables @@ -106,12 +106,12 @@ class SteamCATrajPoseInterpEval : public TransformEvaluator ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief First (earlier) knot ////////////////////////////////////////////////////////////////////////////////////////////// - SteamCATrajVar::ConstPtr knot1_; + SteamTrajVar::ConstPtr knot1_; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Second (later) knot ////////////////////////////////////////////////////////////////////////////////////////////// - SteamCATrajVar::ConstPtr knot2_; + SteamTrajVar::ConstPtr knot2_; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Interpolation coefficients diff --git a/include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp b/include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp index 3e989a8..aac2a94 100644 --- a/include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp +++ b/include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp @@ -29,8 +29,8 @@ class SteamCATrajPriorFactor : public ErrorEvaluatorX ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor ////////////////////////////////////////////////////////////////////////////////////////////// - SteamCATrajPriorFactor(const SteamCATrajVar::ConstPtr& knot1, - const SteamCATrajVar::ConstPtr& knot2); + SteamCATrajPriorFactor(const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Returns whether or not an evaluator contains unlocked state variables @@ -52,12 +52,12 @@ class SteamCATrajPriorFactor : public ErrorEvaluatorX ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief First (earlier) knot ////////////////////////////////////////////////////////////////////////////////////////////// - SteamCATrajVar::ConstPtr knot1_; + SteamTrajVar::ConstPtr knot1_; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Second (later) knot ////////////////////////////////////////////////////////////////////////////////////////////// - SteamCATrajVar::ConstPtr knot2_; + SteamTrajVar::ConstPtr knot2_; }; } // se3 diff --git a/samples/SimpleBAandCATrajPrior.cpp b/samples/SimpleBAandCATrajPrior.cpp index a73d108..6a9db24 100644 --- a/samples/SimpleBAandCATrajPrior.cpp +++ b/samples/SimpleBAandCATrajPrior.cpp @@ -129,10 +129,6 @@ int main(int argc, char** argv) { } // Setup Trajectory - // steam::se3::SteamCATrajInterface ca_traj(Qc_inv); - // steam::se3::SteamTrajInterface *traj_ptr=&ca_traj; - // steam::se3::SteamTrajInterface traj=&traj_ptr; - steam::se3::SteamCATrajInterface ca_traj(Qc_inv); steam::se3::SteamTrajInterface &traj{ca_traj}; @@ -141,7 +137,10 @@ int main(int argc, char** argv) { TrajStateVar& state = traj_states_ic.at(i); steam::se3::TransformStateEvaluator::Ptr temp = steam::se3::TransformStateEvaluator::MakeShared(state.pose); + // steam::se3::SteamCATrajVar::Ptr traj_var(new steam::se3::SteamCATrajVar(state.time, temp, state.velocity, state.acceleration)); + // traj.add(traj_var); traj.add(state.time, temp, state.velocity, state.acceleration); + } // Lock first pose (otherwise entire solution is 'floating') diff --git a/src/trajectory/SteamTrajVar.cpp b/src/trajectory/SteamTrajVar.cpp index 950ff86..ff93c36 100644 --- a/src/trajectory/SteamTrajVar.cpp +++ b/src/trajectory/SteamTrajVar.cpp @@ -39,6 +39,11 @@ const VectorSpaceStateVar::Ptr& SteamTrajVar::getVelocity() const { return velocity_; } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get acceleration state variable, to be overriden +////////////////////////////////////////////////////////////////////////////////////////////// +const VectorSpaceStateVar::Ptr& SteamTrajVar::getAcceleration() const {} + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get timestamp ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/src/trajectory_ca/SteamCATrajInterface.cpp b/src/trajectory_ca/SteamCATrajInterface.cpp index 59ce3ae..24f186a 100644 --- a/src/trajectory_ca/SteamCATrajInterface.cpp +++ b/src/trajectory_ca/SteamCATrajInterface.cpp @@ -22,14 +22,14 @@ namespace se3 { ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// -void SteamCATrajInterface::add(const SteamCATrajVar::Ptr& knot) { +// void SteamCATrajInterface::add(const SteamTrajVar::Ptr& knot) { - // Todo, check that time does not already exist in map? +// // Todo, check that time does not already exist in map? - // Insert in map - knotMap_.insert(knotMap_.end(), - std::pair(knot->getTime().nanosecs(), knot)); -} +// // Insert in map +// knotMap_.insert(knotMap_.end(), +// std::pair(knot->getTime().nanosecs(), knot)); +// } ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot @@ -70,7 +70,7 @@ TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam } // Get iterator to first element with time equal to or great than 'time' - std::map::const_iterator it1 + std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); // Check if time is passed the last entry @@ -79,7 +79,7 @@ TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam // If we allow extrapolation, return constant-acceleration interpolated entry if (allowExtrapolation_) { --it1; // should be safe, as we checked that the map was not empty.. - const SteamCATrajVar::Ptr& endKnot = it1->second; + const SteamTrajVar::Ptr& endKnot = it1->second; TransformEvaluator::Ptr T_t_k = ConstAccTransformEvaluator::MakeShared(endKnot->getVelocity(), endKnot->getAcceleration(), @@ -102,7 +102,7 @@ TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam // If we allow extrapolation, return constant-acceleration interpolated entry if (allowExtrapolation_) { - const SteamCATrajVar::Ptr& startKnot = it1->second; + const SteamTrajVar::Ptr& startKnot = it1->second; TransformEvaluator::Ptr T_t_k = ConstAccTransformEvaluator::MakeShared(startKnot->getVelocity(), startKnot->getAcceleration(), @@ -114,7 +114,7 @@ TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam } // Get iterators bounding the time interval - std::map::const_iterator it2 = it1; --it1; + std::map::const_iterator it2 = it1; --it1; if (time <= it1->second->getTime() || time >= it2->second->getTime()) { throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " "should not trigger... report to a STEAM contributor."); @@ -131,7 +131,7 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { } // Get iterator to first element with time equal to or greater than 'time' - std::map::const_iterator it1 + std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); // Check if time is passed the last entry @@ -140,7 +140,7 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { // If we allow extrapolation, return constant-velocity interpolated entry if (allowExtrapolation_) { --it1; // should be safe, as we checked that the map was not empty.. - const SteamCATrajVar::Ptr& endKnot = it1->second; + const SteamTrajVar::Ptr& endKnot = it1->second; return endKnot->getVelocity()->getValue(); } else { throw std::runtime_error("Requested trajectory evaluator at an invalid time."); @@ -149,7 +149,7 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { // Check if we requested time exactly if (it1->second->getTime() == time) { - const SteamCATrajVar::Ptr& knot = it1->second; + const SteamTrajVar::Ptr& knot = it1->second; // return state variable exactly (no interp) return knot->getVelocity()->getValue(); } @@ -158,7 +158,7 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { if (it1 == knotMap_.begin()) { // If we allow extrapolation, return constant-velocity interpolated entry if (allowExtrapolation_) { - const SteamCATrajVar::Ptr& startKnot = it1->second; + const SteamTrajVar::Ptr& startKnot = it1->second; return startKnot->getVelocity()->getValue(); } else { throw std::runtime_error("Requested trajectory evaluator at an invalid time."); @@ -166,7 +166,7 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { } // Get iterators bounding the time interval - std::map::const_iterator it2 = it1; --it1; + std::map::const_iterator it2 = it1; --it1; if (time <= it1->second->getTime() || time >= it2->second->getTime()) { throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " "should not trigger... report to a STEAM contributor."); @@ -253,80 +253,6 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { return xi_it; } -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior -/// should exist on a trajectory, adding a second will overwrite the first. -////////////////////////////////////////////////////////////////////////////////////////////// -void SteamCATrajInterface::addPosePrior(const steam::Time& time, - const lgmath::se3::Transformation& pose, - const Eigen::Matrix& cov) { - - // Check that map is not empty - if (knotMap_.empty()) { - throw std::runtime_error("[GpTrajectory][addPosePrior] map was empty."); - } - - // Try to find knot at same time - std::map::const_iterator it = knotMap_.find(time.nanosecs()); - if (it == knotMap_.end()) { - throw std::runtime_error("[GpTrajectory][addPosePrior] no knot at provided time."); - } - - // Get reference - const SteamCATrajVar::Ptr& knotRef = it->second; - - // Check that the pose is not locked - if(!knotRef->getPose()->isActive()) { - throw std::runtime_error("[GpTrajectory][addPosePrior] tried to add prior to locked pose."); - } - - // Set up loss function, noise model, and error function - steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); - steam::BaseNoiseModel<6>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<6>(cov)); - steam::TransformErrorEval::Ptr errorfunc(new steam::TransformErrorEval(pose, knotRef->getPose())); - - // Create cost term - posePriorFactor_ = steam::WeightedLeastSqCostTerm<6,6>::Ptr( - new steam::WeightedLeastSqCostTerm<6,6>(errorfunc, sharedNoiseModel, sharedLossFunc)); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Add a unary velocity prior factor at a knot time. Note that only a single velocity -/// prior should exist on a trajectory, adding a second will overwrite the first. -////////////////////////////////////////////////////////////////////////////////////////////// -void SteamCATrajInterface::addVelocityPrior(const steam::Time& time, - const Eigen::Matrix& velocity, - const Eigen::Matrix& cov) { - - // Check that map is not empty - if (knotMap_.empty()) { - throw std::runtime_error("[GpTrajectory][addVelocityPrior] map was empty."); - } - - // Try to find knot at same time - std::map::const_iterator it = knotMap_.find(time.nanosecs()); - if (it == knotMap_.end()) { - throw std::runtime_error("[GpTrajectory][addVelocityPrior] no knot at provided time."); - } - - // Get reference - const SteamCATrajVar::Ptr& knotRef = it->second; - - // Check that the pose is not locked - if(knotRef->getVelocity()->isLocked()) { - throw std::runtime_error("[GpTrajectory][addVelocityPrior] tried to add prior to locked velocity."); - } - - // Set up loss function, noise model, and error function - steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); - steam::BaseNoiseModel<6>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<6>(cov)); - steam::VectorSpaceErrorEval<6,6>::Ptr errorfunc(new steam::VectorSpaceErrorEval<6,6>(velocity, knotRef->getVelocity())); - - // Create cost term - velocityPriorFactor_ = steam::WeightedLeastSqCostTerm<6,6>::Ptr( - new steam::WeightedLeastSqCostTerm<6,6>(errorfunc, sharedNoiseModel, sharedLossFunc)); -} - ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary acceleration prior factor at a knot time. Note that only a single acceleration /// prior should exist on a trajectory, adding a second will overwrite the first. @@ -341,13 +267,13 @@ void SteamCATrajInterface::addAccelerationPrior(const steam::Time& time, } // Try to find knot at same time - std::map::const_iterator it = knotMap_.find(time.nanosecs()); + std::map::const_iterator it = knotMap_.find(time.nanosecs()); if (it == knotMap_.end()) { throw std::runtime_error("[GpTrajectory][addVelocityPrior] no knot at provided time."); } // Get reference - const SteamCATrajVar::Ptr& knotRef = it->second; + const SteamTrajVar::Ptr& knotRef = it->second; // Check that the pose is not locked if(knotRef->getAcceleration()->isLocked()) { @@ -391,18 +317,18 @@ void SteamCATrajInterface::appendPriorCostTerms( // steam::GemanMcClureLossFunc::Ptr sharedLossFunc(new steam::GemanMcClureLossFunc(1)); // Initialize first iterator - std::map::const_iterator it1 = knotMap_.begin(); + std::map::const_iterator it1 = knotMap_.begin(); if (it1 == knotMap_.end()) { throw std::runtime_error("No knots..."); } // Iterate through all states.. if any are unlocked, supply a prior term - std::map::const_iterator it2 = it1; ++it2; + std::map::const_iterator it2 = it1; ++it2; for (; it2 != knotMap_.end(); ++it1, ++it2) { // Get knots - const SteamCATrajVar::ConstPtr& knot1 = it1->second; - const SteamCATrajVar::ConstPtr& knot2 = it2->second; + const SteamTrajVar::ConstPtr& knot1 = it1->second; + const SteamTrajVar::ConstPtr& knot2 = it2->second; // Check if any of the variables are unlocked if(knot1->getPose()->isActive() || !knot1->getVelocity()->isLocked() || @@ -446,7 +372,7 @@ void SteamCATrajInterface::appendPriorCostTerms( // std::map* outStates) const { // // Iterate over trajectory -// std::map::const_iterator it; +// std::map::const_iterator it; // for (it = knotMap_.begin(); it != knotMap_.end(); ++it) { // // Append active states in transform evaluator diff --git a/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp b/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp index 08650a6..d4c6f14 100644 --- a/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp +++ b/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp @@ -15,8 +15,8 @@ namespace se3 { /// \brief Constructor ////////////////////////////////////////////////////////////////////////////////////////////// SteamCATrajPoseInterpEval::SteamCATrajPoseInterpEval(const Time& time, - const SteamCATrajVar::ConstPtr& knot1, - const SteamCATrajVar::ConstPtr& knot2) : + const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2) : knot1_(knot1), knot2_(knot2) { // Calculate time constants @@ -63,8 +63,8 @@ SteamCATrajPoseInterpEval::SteamCATrajPoseInterpEval(const Time& time, /// \brief Pseudo constructor - return a shared pointer to a new instance ////////////////////////////////////////////////////////////////////////////////////////////// SteamCATrajPoseInterpEval::Ptr SteamCATrajPoseInterpEval::MakeShared(const Time& time, - const SteamCATrajVar::ConstPtr& knot1, - const SteamCATrajVar::ConstPtr& knot2) { + const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2) { return SteamCATrajPoseInterpEval::Ptr(new SteamCATrajPoseInterpEval(time, knot1, knot2)); } diff --git a/src/trajectory_ca/SteamCATrajPriorFactor.cpp b/src/trajectory_ca/SteamCATrajPriorFactor.cpp index 0f58b49..c19d5ef 100644 --- a/src/trajectory_ca/SteamCATrajPriorFactor.cpp +++ b/src/trajectory_ca/SteamCATrajPriorFactor.cpp @@ -14,8 +14,8 @@ namespace se3 { ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Constructor ////////////////////////////////////////////////////////////////////////////////////////////// -SteamCATrajPriorFactor::SteamCATrajPriorFactor(const SteamCATrajVar::ConstPtr& knot1, - const SteamCATrajVar::ConstPtr& knot2) : +SteamCATrajPriorFactor::SteamCATrajPriorFactor(const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2) : knot1_(knot1), knot2_(knot2) { } From 06d5708330e247348c4cbb8b912b2be2bf592ed2 Mon Sep 17 00:00:00 2001 From: Jeremy Wong Date: Thu, 14 May 2020 11:36:28 -0400 Subject: [PATCH 25/31] remove unused commented code --- .../steam/trajectory/SteamTrajInterface.hpp | 10 ++++++ .../trajectory_ca/SteamCATrajInterface.hpp | 15 --------- samples/SimpleBAandCATrajPrior.cpp | 6 ++-- src/trajectory/SteamTrajInterface.cpp | 24 ++++++++++---- src/trajectory/SteamTrajVar.cpp | 4 ++- src/trajectory_ca/SteamCATrajInterface.cpp | 32 ------------------- 6 files changed, 33 insertions(+), 58 deletions(-) diff --git a/include/steam/trajectory/SteamTrajInterface.hpp b/include/steam/trajectory/SteamTrajInterface.hpp index eccee76..3dd9069 100644 --- a/include/steam/trajectory/SteamTrajInterface.hpp +++ b/include/steam/trajectory/SteamTrajInterface.hpp @@ -50,6 +50,9 @@ class SteamTrajInterface void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, const VectorSpaceStateVar::Ptr& velocity); + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Add a new knot + ////////////////////////////////////////////////////////////////////////////////////////////// virtual void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, const VectorSpaceStateVar::Ptr& velocity, const VectorSpaceStateVar::Ptr& acceleration); @@ -89,7 +92,14 @@ class SteamTrajInterface virtual void getActiveStateVariables( std::map* outStates) const; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get pose prior cost + ////////////////////////////////////////////////////////////////////////////////////////////// double getPosePriorCost(); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get velocity prior cost + ////////////////////////////////////////////////////////////////////////////////////////////// double getVelocityPriorCost(); protected: diff --git a/include/steam/trajectory_ca/SteamCATrajInterface.hpp b/include/steam/trajectory_ca/SteamCATrajInterface.hpp index dbc3a16..61e8814 100644 --- a/include/steam/trajectory_ca/SteamCATrajInterface.hpp +++ b/include/steam/trajectory_ca/SteamCATrajInterface.hpp @@ -30,11 +30,6 @@ class SteamCATrajInterface : public SteamTrajInterface using SteamTrajInterface::SteamTrajInterface; - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Add a new knot - ////////////////////////////////////////////////////////////////////////////////////////////// - // void add(const SteamCATrajVar::Ptr& knot); - ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// @@ -64,11 +59,6 @@ class SteamCATrajInterface : public SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// void appendPriorCostTerms(const ParallelizedCostTermCollection::Ptr& costTerms) const; - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Get active state variables in the trajectory - ////////////////////////////////////////////////////////////////////////////////////////////// - // void getActiveStateVariables( - // std::map* outStates) const; private: @@ -77,11 +67,6 @@ class SteamCATrajInterface : public SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// steam::WeightedLeastSqCostTerm<6,6>::Ptr accelerationPriorFactor_; - ////////////////////////////////////////////////////////////////////////////////////////////// - /// \brief Ordered map of knots - ////////////////////////////////////////////////////////////////////////////////////////////// - // std::map knotMap_; - }; } // se3 diff --git a/samples/SimpleBAandCATrajPrior.cpp b/samples/SimpleBAandCATrajPrior.cpp index 6a9db24..b200954 100644 --- a/samples/SimpleBAandCATrajPrior.cpp +++ b/samples/SimpleBAandCATrajPrior.cpp @@ -137,9 +137,9 @@ int main(int argc, char** argv) { TrajStateVar& state = traj_states_ic.at(i); steam::se3::TransformStateEvaluator::Ptr temp = steam::se3::TransformStateEvaluator::MakeShared(state.pose); - // steam::se3::SteamCATrajVar::Ptr traj_var(new steam::se3::SteamCATrajVar(state.time, temp, state.velocity, state.acceleration)); - // traj.add(traj_var); - traj.add(state.time, temp, state.velocity, state.acceleration); + steam::se3::SteamCATrajVar::Ptr traj_var(new steam::se3::SteamCATrajVar(state.time, temp, state.velocity, state.acceleration)); + traj.add(traj_var); + // traj.add(state.time, temp, state.velocity, state.acceleration); } diff --git a/src/trajectory/SteamTrajInterface.cpp b/src/trajectory/SteamTrajInterface.cpp index 94d7176..ccdd904 100644 --- a/src/trajectory/SteamTrajInterface.cpp +++ b/src/trajectory/SteamTrajInterface.cpp @@ -27,6 +27,17 @@ SteamTrajInterface::SteamTrajInterface(bool allowExtrapolation) : Qc_inv_(Eigen::Matrix::Identity()), allowExtrapolation_(allowExtrapolation) { } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +SteamTrajInterface::SteamTrajInterface(const Eigen::Matrix& Qc_inv, + bool allowExtrapolation) : + Qc_inv_(Qc_inv), allowExtrapolation_(allowExtrapolation) { +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get pose prior cost +////////////////////////////////////////////////////////////////////////////////////////////// double SteamTrajInterface::getPosePriorCost() { if(posePriorFactor_ != nullptr) { return posePriorFactor_->cost(); @@ -35,6 +46,9 @@ double SteamTrajInterface::getPosePriorCost() { } } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get velocity prior cost +////////////////////////////////////////////////////////////////////////////////////////////// double SteamTrajInterface::getVelocityPriorCost() { if(velocityPriorFactor_ != nullptr) { return velocityPriorFactor_->cost(); @@ -42,13 +56,6 @@ double SteamTrajInterface::getVelocityPriorCost() { return 0.0; } } -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Constructor -////////////////////////////////////////////////////////////////////////////////////////////// -SteamTrajInterface::SteamTrajInterface(const Eigen::Matrix& Qc_inv, - bool allowExtrapolation) : - Qc_inv_(Qc_inv), allowExtrapolation_(allowExtrapolation) { -} ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot @@ -84,6 +91,9 @@ void SteamTrajInterface::add(const steam::Time& time, std::pair(time.nanosecs(), newEntry)); } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Add a new knot +////////////////////////////////////////////////////////////////////////////////////////////// void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, const VectorSpaceStateVar::Ptr& velocity, const VectorSpaceStateVar::Ptr& acceleration) { diff --git a/src/trajectory/SteamTrajVar.cpp b/src/trajectory/SteamTrajVar.cpp index ff93c36..6d7d346 100644 --- a/src/trajectory/SteamTrajVar.cpp +++ b/src/trajectory/SteamTrajVar.cpp @@ -42,7 +42,9 @@ const VectorSpaceStateVar::Ptr& SteamTrajVar::getVelocity() const { ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get acceleration state variable, to be overriden ////////////////////////////////////////////////////////////////////////////////////////////// -const VectorSpaceStateVar::Ptr& SteamTrajVar::getAcceleration() const {} +const VectorSpaceStateVar::Ptr& SteamTrajVar::getAcceleration() const { + throw std::runtime_error("Steam trajectory variable does not have an acceleration state!"); +} ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get timestamp diff --git a/src/trajectory_ca/SteamCATrajInterface.cpp b/src/trajectory_ca/SteamCATrajInterface.cpp index 24f186a..df8b9ba 100644 --- a/src/trajectory_ca/SteamCATrajInterface.cpp +++ b/src/trajectory_ca/SteamCATrajInterface.cpp @@ -19,18 +19,6 @@ namespace steam { namespace se3 { -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Add a new knot -////////////////////////////////////////////////////////////////////////////////////////////// -// void SteamCATrajInterface::add(const SteamTrajVar::Ptr& knot) { - -// // Todo, check that time does not already exist in map? - -// // Insert in map -// knotMap_.insert(knotMap_.end(), -// std::pair(knot->getTime().nanosecs(), knot)); -// } - ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// @@ -365,25 +353,5 @@ void SteamCATrajInterface::appendPriorCostTerms( } } -// ////////////////////////////////////////////////////////////////////////////////////////////// -// /// \brief Get active state variables in the trajectory -// ////////////////////////////////////////////////////////////////////////////////////////////// -// void SteamCATrajInterface::getActiveStateVariables( -// std::map* outStates) const { - -// // Iterate over trajectory -// std::map::const_iterator it; -// for (it = knotMap_.begin(); it != knotMap_.end(); ++it) { - -// // Append active states in transform evaluator -// it->second->getPose()->getActiveStateVariables(outStates); - -// // Check if velocity is locked -// if (!it->second->getVelocity()->isLocked()) { -// (*outStates)[it->second->getVelocity()->getKey().getID()] = it->second->getVelocity(); -// } -// } -// } - } // se3 } // steam From b3d392a32c653ec15f29956f4416a3386a047a17 Mon Sep 17 00:00:00 2001 From: Jeremy Wong Date: Fri, 15 May 2020 10:20:24 -0400 Subject: [PATCH 26/31] remove trajectory_ca folder and move everything to trajectory --- include/steam.hpp | 2 +- .../{trajectory_ca => trajectory}/SteamCATrajInterface.hpp | 2 +- .../SteamCATrajPoseInterpEval.hpp | 2 +- .../SteamCATrajPriorFactor.hpp | 2 +- .../steam/{trajectory_ca => trajectory}/SteamCATrajVar.hpp | 0 src/{trajectory_ca => trajectory}/SteamCATrajInterface.cpp | 6 +++--- .../SteamCATrajPoseInterpEval.cpp | 2 +- .../SteamCATrajPriorFactor.cpp | 2 +- src/{trajectory_ca => trajectory}/SteamCATrajVar.cpp | 2 +- 9 files changed, 10 insertions(+), 10 deletions(-) rename include/steam/{trajectory_ca => trajectory}/SteamCATrajInterface.hpp (98%) rename include/steam/{trajectory_ca => trajectory}/SteamCATrajPoseInterpEval.hpp (99%) rename include/steam/{trajectory_ca => trajectory}/SteamCATrajPriorFactor.hpp (98%) rename include/steam/{trajectory_ca => trajectory}/SteamCATrajVar.hpp (100%) rename src/{trajectory_ca => trajectory}/SteamCATrajInterface.cpp (98%) rename src/{trajectory_ca => trajectory}/SteamCATrajPoseInterpEval.cpp (99%) rename src/{trajectory_ca => trajectory}/SteamCATrajPriorFactor.cpp (99%) rename src/{trajectory_ca => trajectory}/SteamCATrajVar.cpp (96%) diff --git a/include/steam.hpp b/include/steam.hpp index 97fbd77..51b0b00 100644 --- a/include/steam.hpp +++ b/include/steam.hpp @@ -61,6 +61,6 @@ // trajectory #include -#include +#include #endif // STEAM_ESTIMATION_LIBRARY_HPP diff --git a/include/steam/trajectory_ca/SteamCATrajInterface.hpp b/include/steam/trajectory/SteamCATrajInterface.hpp similarity index 98% rename from include/steam/trajectory_ca/SteamCATrajInterface.hpp rename to include/steam/trajectory/SteamCATrajInterface.hpp index 61e8814..808899f 100644 --- a/include/steam/trajectory_ca/SteamCATrajInterface.hpp +++ b/include/steam/trajectory/SteamCATrajInterface.hpp @@ -11,7 +11,7 @@ #include -#include +#include #include #include diff --git a/include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp b/include/steam/trajectory/SteamCATrajPoseInterpEval.hpp similarity index 99% rename from include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp rename to include/steam/trajectory/SteamCATrajPoseInterpEval.hpp index 9165d7a..9974c32 100644 --- a/include/steam/trajectory_ca/SteamCATrajPoseInterpEval.hpp +++ b/include/steam/trajectory/SteamCATrajPoseInterpEval.hpp @@ -9,7 +9,7 @@ #include -#include +#include #include namespace steam { diff --git a/include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp b/include/steam/trajectory/SteamCATrajPriorFactor.hpp similarity index 98% rename from include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp rename to include/steam/trajectory/SteamCATrajPriorFactor.hpp index aac2a94..7738e0a 100644 --- a/include/steam/trajectory_ca/SteamCATrajPriorFactor.hpp +++ b/include/steam/trajectory/SteamCATrajPriorFactor.hpp @@ -9,7 +9,7 @@ #include -#include +#include #include namespace steam { diff --git a/include/steam/trajectory_ca/SteamCATrajVar.hpp b/include/steam/trajectory/SteamCATrajVar.hpp similarity index 100% rename from include/steam/trajectory_ca/SteamCATrajVar.hpp rename to include/steam/trajectory/SteamCATrajVar.hpp diff --git a/src/trajectory_ca/SteamCATrajInterface.cpp b/src/trajectory/SteamCATrajInterface.cpp similarity index 98% rename from src/trajectory_ca/SteamCATrajInterface.cpp rename to src/trajectory/SteamCATrajInterface.cpp index df8b9ba..4ebb8b6 100644 --- a/src/trajectory_ca/SteamCATrajInterface.cpp +++ b/src/trajectory/SteamCATrajInterface.cpp @@ -4,12 +4,12 @@ /// \author Tim Tang, ASRL ////////////////////////////////////////////////////////////////////////////////////////////// -#include +#include #include -#include -#include +#include +#include #include #include diff --git a/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp b/src/trajectory/SteamCATrajPoseInterpEval.cpp similarity index 99% rename from src/trajectory_ca/SteamCATrajPoseInterpEval.cpp rename to src/trajectory/SteamCATrajPoseInterpEval.cpp index d4c6f14..40f160c 100644 --- a/src/trajectory_ca/SteamCATrajPoseInterpEval.cpp +++ b/src/trajectory/SteamCATrajPoseInterpEval.cpp @@ -4,7 +4,7 @@ /// \author Tim Tang, ASRL ////////////////////////////////////////////////////////////////////////////////////////////// -#include +#include #include diff --git a/src/trajectory_ca/SteamCATrajPriorFactor.cpp b/src/trajectory/SteamCATrajPriorFactor.cpp similarity index 99% rename from src/trajectory_ca/SteamCATrajPriorFactor.cpp rename to src/trajectory/SteamCATrajPriorFactor.cpp index c19d5ef..49f77ea 100644 --- a/src/trajectory_ca/SteamCATrajPriorFactor.cpp +++ b/src/trajectory/SteamCATrajPriorFactor.cpp @@ -4,7 +4,7 @@ /// \author Tim Tang, ASRL ////////////////////////////////////////////////////////////////////////////////////////////// -#include +#include #include diff --git a/src/trajectory_ca/SteamCATrajVar.cpp b/src/trajectory/SteamCATrajVar.cpp similarity index 96% rename from src/trajectory_ca/SteamCATrajVar.cpp rename to src/trajectory/SteamCATrajVar.cpp index 04bd632..874f21d 100644 --- a/src/trajectory_ca/SteamCATrajVar.cpp +++ b/src/trajectory/SteamCATrajVar.cpp @@ -4,7 +4,7 @@ /// \author Tim Tang, ASRL ////////////////////////////////////////////////////////////////////////////////////////////// -#include +#include #include From 2707a87a92231c9576719cb5173aab6b0f188161 Mon Sep 17 00:00:00 2001 From: Jeremy Wong Date: Thu, 26 Sep 2019 16:07:48 -0400 Subject: [PATCH 27/31] cherry pick velocity extrapolation fix --- src/trajectory/SteamCATrajInterface.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/trajectory/SteamCATrajInterface.cpp b/src/trajectory/SteamCATrajInterface.cpp index 4ebb8b6..b51ed60 100644 --- a/src/trajectory/SteamCATrajInterface.cpp +++ b/src/trajectory/SteamCATrajInterface.cpp @@ -128,8 +128,14 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { // If we allow extrapolation, return constant-velocity interpolated entry if (allowExtrapolation_) { --it1; // should be safe, as we checked that the map was not empty.. + const SteamTrajVar::Ptr& endKnot = it1->second; - return endKnot->getVelocity()->getValue(); + + auto dt=(time-endKnot->getTime()).seconds(); + auto xi_tk=endKnot->getVelocity()->getValue()*dt+0.5*dt*dt*endKnot->getAcceleration()->getValue(); + + Eigen::Matrix J_tk = lgmath::se3::vec2jac(xi_tk); + return J_tk*(endKnot->getVelocity()->getValue()+endKnot->getAcceleration()->getValue()*dt); } else { throw std::runtime_error("Requested trajectory evaluator at an invalid time."); } From d5fab66937f648f34a1380352ffe48d8ef305c56 Mon Sep 17 00:00:00 2001 From: Jeremy Wong Date: Sat, 27 Jun 2020 15:12:02 -0400 Subject: [PATCH 28/31] Add imu factor to steam --- include/steam.hpp | 2 + .../steam/evaluator/samples/ImuErrorEval.hpp | 76 +++++++++++++ .../samples/LinearFuncErrorEval-inl.hpp | 69 ++++++++++++ .../evaluator/samples/LinearFuncErrorEval.hpp | 76 +++++++++++++ src/evaluator/samples/ImuErrorEval.cpp | 104 ++++++++++++++++++ src/trajectory/SteamCATrajInterface.cpp | 1 - src/trajectory/SteamTrajInterface.cpp | 6 - 7 files changed, 327 insertions(+), 7 deletions(-) create mode 100644 include/steam/evaluator/samples/ImuErrorEval.hpp create mode 100644 include/steam/evaluator/samples/LinearFuncErrorEval-inl.hpp create mode 100644 include/steam/evaluator/samples/LinearFuncErrorEval.hpp create mode 100644 src/evaluator/samples/ImuErrorEval.cpp diff --git a/include/steam.hpp b/include/steam.hpp index 51b0b00..de57eea 100644 --- a/include/steam.hpp +++ b/include/steam.hpp @@ -35,6 +35,8 @@ #include #include #include +#include +#include #include // evaluator - block auto diff diff --git a/include/steam/evaluator/samples/ImuErrorEval.hpp b/include/steam/evaluator/samples/ImuErrorEval.hpp new file mode 100644 index 0000000..6fcdee4 --- /dev/null +++ b/include/steam/evaluator/samples/ImuErrorEval.hpp @@ -0,0 +1,76 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file ImuErrorEval.hpp +/// +/// \author Sean Anderson, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_IMU_ERROR_EVALUATOR_HPP +#define STEAM_IMU_ERROR_EVALUATOR_HPP + +#include + +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Error evaluator for a measured vector space state variable +////////////////////////////////////////////////////////////////////////////////////////////// +class ImuErrorEval : public ErrorEvaluator<6,6>::type +{ +public: + + /// Convenience typedefs + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + ImuErrorEval(const Eigen::Matrix& measurement, + const Eigen::Matrix3d& C_body_enu, + const VectorSpaceStateVar::ConstPtr& varpi, + const VectorSpaceStateVar::ConstPtr& varpi_dot, + const VectorSpaceStateVar::ConstPtr& imu_bias, + const lgmath::se3::Transformation& T_s_v); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the measurement error + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Matrix evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the measurement error and relevant Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Matrix evaluate( + const Eigen::Matrix& lhs, + std::vector >* jacs) const; + +private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Measurement vector + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::Matrix meas_; + + //////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Vectorspace state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::ConstPtr varpi_; + VectorSpaceStateVar::ConstPtr varpi_dot_; + VectorSpaceStateVar::ConstPtr imu_bias_; + + Eigen::Matrix3d C_body_enu_; + + Eigen::Vector3d gravity_; + + Eigen::Matrix adT_s_v_; + +}; + +} // steam + +#endif // STEAM_IMU_ERROR_EVALUATOR_HPP diff --git a/include/steam/evaluator/samples/LinearFuncErrorEval-inl.hpp b/include/steam/evaluator/samples/LinearFuncErrorEval-inl.hpp new file mode 100644 index 0000000..3120d3c --- /dev/null +++ b/include/steam/evaluator/samples/LinearFuncErrorEval-inl.hpp @@ -0,0 +1,69 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file LinearFuncErrorEval-inl.hpp +/// +/// \author Sean Anderson, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +template +LinearFuncErrorEval::LinearFuncErrorEval( + const Eigen::Matrix& measurement, + const Eigen::Matrix& C, + const VectorSpaceStateVar::ConstPtr& stateVec) + : measurement_(measurement), C_(C), stateVec_(stateVec) { +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool LinearFuncErrorEval::isActive() const { + return !stateVec_->isLocked(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the measurement error +////////////////////////////////////////////////////////////////////////////////////////////// +template +Eigen::Matrix LinearFuncErrorEval::evaluate() const { + return measurement_ - C_*(stateVec_->getValue()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the measurement error and relevant Jacobians +////////////////////////////////////////////////////////////////////////////////////////////// +template +Eigen::Matrix LinearFuncErrorEval::evaluate( + const Eigen::Matrix& lhs, + std::vector >* jacs) const { + + // Check and initialize jacobian array + if (jacs == NULL) { + throw std::invalid_argument("Null pointer provided to return-input 'jacs' in evaluate"); + } + jacs->clear(); + + // Check that dimensions match + // if (lhs.cols() != stateVec_->getPerturbDim()) { + // throw std::runtime_error("evaluate had dimension mismatch."); + // } + + // Construct Jacobian + if(!stateVec_->isLocked()) { + jacs->resize(1); + Jacobian& jacref = jacs->back(); + jacref.key = stateVec_->getKey(); + jacref.jac = -lhs*C_; + } + + // Return error + return measurement_ - C_*(stateVec_->getValue()); +} + +} // steam diff --git a/include/steam/evaluator/samples/LinearFuncErrorEval.hpp b/include/steam/evaluator/samples/LinearFuncErrorEval.hpp new file mode 100644 index 0000000..f5b8751 --- /dev/null +++ b/include/steam/evaluator/samples/LinearFuncErrorEval.hpp @@ -0,0 +1,76 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file LinearFuncErrorEval.hpp +/// +/// \author Sean Anderson, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_LINEAR_FUNC_ERROR_EVALUATOR_HPP +#define STEAM_LINEAR_FUNC_ERROR_EVALUATOR_HPP + +#include + +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Error evaluator for a measured vector space state variable +////////////////////////////////////////////////////////////////////////////////////////////// +template +class LinearFuncErrorEval : public ErrorEvaluator::type +{ +public: + + /// Convenience typedefs + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + LinearFuncErrorEval(const Eigen::Matrix& measurement, + const Eigen::Matrix& C, + const VectorSpaceStateVar::ConstPtr& stateVec); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the measurement error + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Matrix evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the measurement error and relevant Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Matrix evaluate( + const Eigen::Matrix& lhs, + std::vector >* jacs) const; + +private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Measurement vector + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::Matrix measurement_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Vectorspace state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::ConstPtr stateVec_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Matrix that transforms state vector into measurement + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::Matrix C_; + + +}; + +typedef LinearFuncErrorEval LinearFuncErrorEvalX; + +} // steam + +#include + +#endif // STEAM_VECTOR_SPACE_ERROR_EVALUATOR_HPP diff --git a/src/evaluator/samples/ImuErrorEval.cpp b/src/evaluator/samples/ImuErrorEval.cpp new file mode 100644 index 0000000..ecce872 --- /dev/null +++ b/src/evaluator/samples/ImuErrorEval.cpp @@ -0,0 +1,104 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file ImuErrorEval.cpp +/// +/// \author Sean Anderson, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +ImuErrorEval::ImuErrorEval( + const Eigen::Matrix& measurement, + const Eigen::Matrix3d& C_body_enu, + const VectorSpaceStateVar::ConstPtr& varpi, + const VectorSpaceStateVar::ConstPtr& varpi_dot, + const VectorSpaceStateVar::ConstPtr& imu_bias, + const lgmath::se3::Transformation& T_s_v) + : meas_(measurement), C_body_enu_(C_body_enu), varpi_(varpi), varpi_dot_(varpi_dot), imu_bias_(imu_bias), adT_s_v_(T_s_v.adjoint()) { + gravity_=Eigen::Vector3d::Zero(); + gravity_(2)=9.81; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool ImuErrorEval::isActive() const { + return !varpi_->isLocked() || !varpi_dot_->isLocked() || !imu_bias_->isLocked(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the measurement error +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Matrix ImuErrorEval::evaluate() const { + Eigen::Matrix error; + error.head<3>() = meas_.head<3>() + adT_s_v_.bottomRightCorner<3,3>()*varpi_->getValue().tail<3>() - imu_bias_->getValue().head<3>(); + error.tail<3>() = meas_.tail<3>() + adT_s_v_.topLeftCorner<3,3>()*varpi_dot_->getValue().head<3>() + adT_s_v_.topRightCorner<3,3>()*varpi_->getValue().tail<3>() + - imu_bias_->getValue().tail<3>() - C_body_enu_*gravity_; + return error; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the measurement error and relevant Jacobians +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Matrix ImuErrorEval::evaluate( + const Eigen::Matrix& lhs, + std::vector >* jacs) const { + + // Check and initialize jacobian array + if (jacs == NULL) { + throw std::invalid_argument("Null pointer provided to return-input 'jacs' in evaluate"); + } + jacs->clear(); + + // Check that dimensions match + // if (lhs.cols() != stateVec_->getPerturbDim()) { + // throw std::runtime_error("evaluate had dimension mismatch."); + // } + + // Construct Jacobian + if(!varpi_->isLocked()) { + // Construct Jacobian Object + jacs->push_back(Jacobian<6,6>()); + Jacobian<6,6>& jacref = jacs->back(); + jacref.key = varpi_->getKey(); + Eigen::MatrixXd jacobian(6,6); + jacobian.setZero(); + jacobian.topRightCorner<3,3>()=adT_s_v_.bottomRightCorner<3,3>();//Eigen::Matrix3d::Identity(); + jacref.jac = lhs * jacobian; + } + + if(!varpi_dot_->isLocked()) { + // Construct Jacobian Object + jacs->push_back(Jacobian<6,6>()); + Jacobian<6,6>& jacref = jacs->back(); + jacref.key = varpi_dot_->getKey(); + Eigen::MatrixXd jacobian(6,6); + jacobian.setZero(); + jacobian.bottomLeftCorner<3,3>()=adT_s_v_.topLeftCorner<3,3>();//Eigen::Matrix3d::Identity(); + jacobian.bottomRightCorner<3,3>()=adT_s_v_.topRightCorner<3,3>(); + jacref.jac = lhs * jacobian; + } + + if(!imu_bias_->isLocked()) { + // Construct Jacobian Object + jacs->push_back(Jacobian<6,6>()); + Jacobian<6,6>& jacref = jacs->back(); + jacref.key = imu_bias_->getKey(); + Eigen::MatrixXd jacobian(6,6); + jacobian.setIdentity(); + jacref.jac = -lhs * jacobian; + } + + + // Return error + Eigen::Matrix error; + error.head<3>() = meas_.head<3>() + adT_s_v_.bottomRightCorner<3,3>()*varpi_->getValue().tail<3>() - imu_bias_->getValue().head<3>(); + error.tail<3>() = meas_.tail<3>() + adT_s_v_.topLeftCorner<3,3>()*varpi_dot_->getValue().head<3>() + adT_s_v_.topRightCorner<3,3>()*varpi_->getValue().tail<3>() + - imu_bias_->getValue().tail<3>() - C_body_enu_*gravity_; + return error; +} + +} // steam diff --git a/src/trajectory/SteamCATrajInterface.cpp b/src/trajectory/SteamCATrajInterface.cpp index b51ed60..1ac6d36 100644 --- a/src/trajectory/SteamCATrajInterface.cpp +++ b/src/trajectory/SteamCATrajInterface.cpp @@ -51,7 +51,6 @@ void SteamCATrajInterface::add(const steam::Time& time, /// \brief Get evaluator ////////////////////////////////////////////////////////////////////////////////////////////// TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam::Time& time) const { - // Check that map is not empty if (knotMap_.empty()) { throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); diff --git a/src/trajectory/SteamTrajInterface.cpp b/src/trajectory/SteamTrajInterface.cpp index ccdd904..d4fa1a4 100644 --- a/src/trajectory/SteamTrajInterface.cpp +++ b/src/trajectory/SteamTrajInterface.cpp @@ -104,16 +104,13 @@ void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvalua /// \brief Get evaluator ////////////////////////////////////////////////////////////////////////////////////////////// TransformEvaluator::ConstPtr SteamTrajInterface::getInterpPoseEval(const steam::Time& time) const { - // Check that map is not empty if (knotMap_.empty()) { throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); } - // Get iterator to first element with time equal to or greater than 'time' std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); - // Check if time is passed the last entry if (it1 == knotMap_.end()) { @@ -128,7 +125,6 @@ TransformEvaluator::ConstPtr SteamTrajInterface::getInterpPoseEval(const steam:: throw std::runtime_error("Requested trajectory evaluator at an invalid time."); } } - // Check if we requested time exactly if (it1->second->getTime() == time) { @@ -150,14 +146,12 @@ TransformEvaluator::ConstPtr SteamTrajInterface::getInterpPoseEval(const steam:: throw std::runtime_error("Requested trajectory evaluator at an invalid time."); } } - // Get iterators bounding the time interval std::map::const_iterator it2 = it1; --it1; if (time <= it1->second->getTime() || time >= it2->second->getTime()) { throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " "should not trigger... report to a STEAM contributor."); } - // Create interpolated evaluator return SteamTrajPoseInterpEval::MakeShared(time, it1->second, it2->second); } From abb785c230418ff04984768b3b67fd2b53f615cf Mon Sep 17 00:00:00 2001 From: Jeremy Wong Date: Sat, 29 Aug 2020 08:47:08 -0400 Subject: [PATCH 29/31] add get acceleration function --- .../steam/trajectory/SteamCATrajInterface.hpp | 5 +++ src/trajectory/SteamCATrajInterface.cpp | 42 +++++++++++++++++++ 2 files changed, 47 insertions(+) diff --git a/include/steam/trajectory/SteamCATrajInterface.hpp b/include/steam/trajectory/SteamCATrajInterface.hpp index 808899f..e45ebe5 100644 --- a/include/steam/trajectory/SteamCATrajInterface.hpp +++ b/include/steam/trajectory/SteamCATrajInterface.hpp @@ -47,6 +47,11 @@ class SteamCATrajInterface : public SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// Eigen::VectorXd getVelocity(const steam::Time& time); + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get acceleration evaluator + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::VectorXd getAcceleration(const steam::Time& time); + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary acceleration prior factor at a knot time. Note that only a single acceleration /// prior should exist on a trajectory, adding a second will overwrite the first. diff --git a/src/trajectory/SteamCATrajInterface.cpp b/src/trajectory/SteamCATrajInterface.cpp index 1ac6d36..cbdc4e3 100644 --- a/src/trajectory/SteamCATrajInterface.cpp +++ b/src/trajectory/SteamCATrajInterface.cpp @@ -246,6 +246,48 @@ Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { return xi_it; } + Eigen::VectorXd SteamCATrajInterface::getAcceleration(const steam::Time& time) { + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); + } + + // Get iterator to first element with time equal to or greater than 'time' + std::map::const_iterator it1 + = knotMap_.lower_bound(time.nanosecs()); + + // Check if time is passed the last entry + if (it1 == knotMap_.end()) { + + // If we allow extrapolation, return constant-velocity interpolated entry + if (allowExtrapolation_) { + --it1; // should be safe, as we checked that the map was not empty.. + const SteamTrajVar::Ptr& endKnot = it1->second; + return endKnot->getAcceleration()->getValue(); + } else { + throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + } + } + + // Check if we requested time exactly + // if (it1->second->getTime() == time) { + const SteamTrajVar::Ptr& knot = it1->second; + // return state variable exactly (no interp) + return knot->getAcceleration()->getValue(); + // } + + // // Check if we requested before first time + // if (it1 == knotMap_.begin()) { + // // If we allow extrapolation, return constant-velocity interpolated entry + // if (allowExtrapolation_) { + // const SteamTrajVar::Ptr& startKnot = it1->second; + // return startKnot->getAcceleration()->getValue(); + // } else { + // throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + // } + // } + } + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary acceleration prior factor at a knot time. Note that only a single acceleration /// prior should exist on a trajectory, adding a second will overwrite the first. From 2f938a7b84852112c51e52a9284d3bdaaa2cbb6f Mon Sep 17 00:00:00 2001 From: Jeremy Wong Date: Wed, 30 Sep 2020 23:39:47 -0400 Subject: [PATCH 30/31] support storaing the m600 imu covariance in the trajectory --- .../steam/trajectory/SteamCATrajInterface.hpp | 10 ++ include/steam/trajectory/SteamCATrajVar.hpp | 5 + .../steam/trajectory/SteamTrajInterface.hpp | 26 ++++ include/steam/trajectory/SteamTrajVar.hpp | 11 ++ src/trajectory/SteamCATrajInterface.cpp | 80 +++++++++++++ src/trajectory/SteamCATrajVar.cpp | 7 ++ src/trajectory/SteamTrajInterface.cpp | 112 ++++++++++++++++++ src/trajectory/SteamTrajVar.cpp | 14 +++ 8 files changed, 265 insertions(+) diff --git a/include/steam/trajectory/SteamCATrajInterface.hpp b/include/steam/trajectory/SteamCATrajInterface.hpp index e45ebe5..dbc22db 100644 --- a/include/steam/trajectory/SteamCATrajInterface.hpp +++ b/include/steam/trajectory/SteamCATrajInterface.hpp @@ -37,6 +37,11 @@ class SteamCATrajInterface : public SteamTrajInterface const VectorSpaceStateVar::Ptr& velocity, const VectorSpaceStateVar::Ptr& acceleration); + void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov); + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get evaluator ////////////////////////////////////////////////////////////////////////////////////////////// @@ -64,6 +69,11 @@ class SteamCATrajInterface : public SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// void appendPriorCostTerms(const ParallelizedCostTermCollection::Ptr& costTerms) const; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get interpolated/extrapolated covariance at given time + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::MatrixXd getCovariance(const steam::Time& time) const; + private: diff --git a/include/steam/trajectory/SteamCATrajVar.hpp b/include/steam/trajectory/SteamCATrajVar.hpp index add3ffa..1de5d9d 100644 --- a/include/steam/trajectory/SteamCATrajVar.hpp +++ b/include/steam/trajectory/SteamCATrajVar.hpp @@ -37,6 +37,11 @@ class SteamCATrajVar : public SteamTrajVar const VectorSpaceStateVar::Ptr& velocity, const VectorSpaceStateVar::Ptr& acceleration); + SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov); + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get velocity state variable ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/include/steam/trajectory/SteamTrajInterface.hpp b/include/steam/trajectory/SteamTrajInterface.hpp index 3dd9069..7474d93 100644 --- a/include/steam/trajectory/SteamTrajInterface.hpp +++ b/include/steam/trajectory/SteamTrajInterface.hpp @@ -16,6 +16,8 @@ #include #include +#include + namespace steam { namespace se3 { @@ -50,6 +52,10 @@ class SteamTrajInterface void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, const VectorSpaceStateVar::Ptr& velocity); + void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const Eigen::Matrix cov); + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// @@ -57,6 +63,11 @@ class SteamTrajInterface const VectorSpaceStateVar::Ptr& velocity, const VectorSpaceStateVar::Ptr& acceleration); + virtual void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov); + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get transform evaluator ////////////////////////////////////////////////////////////////////////////////////////////// @@ -102,6 +113,16 @@ class SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// double getVelocityPriorCost(); + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Store solver in trajectory, needed for querying covariances later + ////////////////////////////////////////////////////////////////////////////////////////////// + void setSolver(std::shared_ptr solver); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get interpolated/extrapolated covariance at given time + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::MatrixXd getCovariance(const steam::Time& time) const; + protected: ////////////////////////////////////////////////////////////////////////////////////////////// @@ -129,6 +150,11 @@ class SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// std::map knotMap_; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Solver used, we use this to query covariances + ////////////////////////////////////////////////////////////////////////////////////////////// + std::shared_ptr solver_; + }; } // se3 diff --git a/include/steam/trajectory/SteamTrajVar.hpp b/include/steam/trajectory/SteamTrajVar.hpp index 2dfd603..e201a6d 100644 --- a/include/steam/trajectory/SteamTrajVar.hpp +++ b/include/steam/trajectory/SteamTrajVar.hpp @@ -34,6 +34,9 @@ class SteamTrajVar SteamTrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, const VectorSpaceStateVar::Ptr& velocity); + SteamTrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, const Eigen::Matrix cov); + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get pose evaluator ////////////////////////////////////////////////////////////////////////////////////////////// @@ -54,6 +57,10 @@ class SteamTrajVar ////////////////////////////////////////////////////////////////////////////////////////////// const steam::Time& getTime() const; + const Eigen::Matrix getCovariance() const; + + bool covarianceSet() const; + private: ////////////////////////////////////////////////////////////////////////////////////////////// @@ -70,6 +77,10 @@ class SteamTrajVar /// \brief Generalized 6D velocity state variable ////////////////////////////////////////////////////////////////////////////////////////////// VectorSpaceStateVar::Ptr velocity_; + + protected: + Eigen::MatrixXd cov_; + bool cov_set_=false; }; } // se3 diff --git a/src/trajectory/SteamCATrajInterface.cpp b/src/trajectory/SteamCATrajInterface.cpp index cbdc4e3..abcc53b 100644 --- a/src/trajectory/SteamCATrajInterface.cpp +++ b/src/trajectory/SteamCATrajInterface.cpp @@ -47,6 +47,32 @@ void SteamCATrajInterface::add(const steam::Time& time, std::pair(time.nanosecs(), newEntry)); } +void SteamCATrajInterface::add(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov) { + + // Check velocity input + if (velocity->getPerturbDim() != 6) { + throw std::invalid_argument("invalid velocity size"); + } + + // Check acceleration input + if (acceleration->getPerturbDim() != 6) { + throw std::invalid_argument("invalid acceleration size"); + } + + // Todo, check that time does not already exist in map? + + // Make knot + SteamCATrajVar::Ptr newEntry(new SteamCATrajVar(time, T_k0, velocity, acceleration, cov)); + + // Insert in map + knotMap_.insert(knotMap_.end(), + std::pair(time.nanosecs(), newEntry)); +} + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get evaluator ////////////////////////////////////////////////////////////////////////////////////////////// @@ -400,5 +426,59 @@ void SteamCATrajInterface::appendPriorCostTerms( } } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get interpolated/extrapolated covariance at given time, for now only support at knot times +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::MatrixXd SteamCATrajInterface::getCovariance(const steam::Time& time) const { + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); + } + + // Get iterator to first element with time equal to or great than 'time' + std::map::const_iterator it1 + = knotMap_.lower_bound(time.nanosecs()); + + // Check if time is passed the last entry + if (it1 == knotMap_.end()) { + --it1; // last knot + // If we allow extrapolation + // if (allowExtrapolation_) { + // return extrapCovariance(solver, time); + // } else { + // throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + // } + } + + // Check if we requested time exactly + // if (it1->second->getTime() == time) { + // return covariance exactly (no interp) + if (it1->second->covarianceSet()) { + return it1->second->getCovariance(); + } + std::map outState; + it1->second->getPose()->getActiveStateVariables(&outState); + + std::vector keys; + keys.push_back(outState.begin()->second->getKey()); + keys.push_back(it1->second->getVelocity()->getKey()); + keys.push_back(it1->second->getAcceleration()->getKey()); + + steam::BlockMatrix covariance = solver_->queryCovarianceBlock(keys); + + Eigen::Matrix output; + output.block<6,6>(0,0) = covariance.copyAt(0,0); + output.block<6,6>(0,6) = covariance.copyAt(0,1); + output.block<6,6>(0,12) = covariance.copyAt(0,2); + output.block<6,6>(6,0) = covariance.copyAt(1,0); + output.block<6,6>(6,6) = covariance.copyAt(1,1); + output.block<6,6>(6,12) = covariance.copyAt(1,2); + output.block<6,6>(12,0) = covariance.copyAt(2,0); + output.block<6,6>(12,6) = covariance.copyAt(2,1); + output.block<6,6>(12,12) = covariance.copyAt(2,2); + return output; + // } +} + } // se3 } // steam diff --git a/src/trajectory/SteamCATrajVar.cpp b/src/trajectory/SteamCATrajVar.cpp index 874f21d..4d692c8 100644 --- a/src/trajectory/SteamCATrajVar.cpp +++ b/src/trajectory/SteamCATrajVar.cpp @@ -31,6 +31,13 @@ SteamCATrajVar::SteamCATrajVar(const steam::Time& time, } } +SteamCATrajVar::SteamCATrajVar(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov) + : SteamCATrajVar(time, T_k0, velocity, acceleration) { cov_set_=true; cov_=cov; } + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get acceleration state variable ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/src/trajectory/SteamTrajInterface.cpp b/src/trajectory/SteamTrajInterface.cpp index d4fa1a4..26714a3 100644 --- a/src/trajectory/SteamTrajInterface.cpp +++ b/src/trajectory/SteamTrajInterface.cpp @@ -91,6 +91,26 @@ void SteamTrajInterface::add(const steam::Time& time, std::pair(time.nanosecs(), newEntry)); } +void SteamTrajInterface::add(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const Eigen::Matrix cov) { + + // Check velocity input + if (velocity->getPerturbDim() != 6) { + throw std::invalid_argument("invalid velocity size"); + } + + // Todo, check that time does not already exist in map? + + // Make knot + SteamTrajVar::Ptr newEntry(new SteamTrajVar(time, T_k0, velocity, cov)); + + // Insert in map + knotMap_.insert(knotMap_.end(), + std::pair(time.nanosecs(), newEntry)); +} + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot ////////////////////////////////////////////////////////////////////////////////////////////// @@ -100,6 +120,13 @@ void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvalua add(time, T_k0, velocity); } +void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov) { + add(time, T_k0, velocity, cov.topLeftCorner<12,12>()); +} + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get evaluator ////////////////////////////////////////////////////////////////////////////////////////////// @@ -389,6 +416,91 @@ void SteamTrajInterface::appendPriorCostTerms( } } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Store solver in trajectory, needed for querying covariances later +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamTrajInterface::setSolver(std::shared_ptr solver) { + solver_ = solver; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get interpolated/extrapolated covariance at given time, for now only support at knot times +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::MatrixXd SteamTrajInterface::getCovariance(const steam::Time& time) const { + + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); + } + + // Get iterator to first element with time equal to or great than 'time' + std::map::const_iterator it1 + = knotMap_.lower_bound(time.nanosecs()); + + // Check if time is passed the last entry + if (it1 == knotMap_.end()) { + --it1; + // If we allow extrapolation + // if (allowExtrapolation_) { + // return extrapCovariance(solver, time); + // } else { + // throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + // } + } + + // Check if we requested time exactly + // if (it1->second->getTime() == time) { + // check if the state variable at time has associated covariacne + if (it1->second->covarianceSet()) { + return it1->second->getCovariance(); + } + // return covariance exactly (no interp) + std::map outState; + it1->second->getPose()->getActiveStateVariables(&outState); + + std::vector keys; + keys.push_back(outState.begin()->second->getKey()); + keys.push_back(it1->second->getVelocity()->getKey()); + + steam::BlockMatrix covariance = solver_->queryCovarianceBlock(keys); + + Eigen::Matrix output; + output.block<6,6>(0,0) = covariance.copyAt(0,0); + output.block<6,6>(0,6) = covariance.copyAt(0,1); + output.block<6,6>(6,0) = covariance.copyAt(1,0); + output.block<6,6>(6,6) = covariance.copyAt(1,1); + return output; + // } + + // // TODO(DAVID): Be able to handle locked states + // // Check if state is locked + // std::map states; + // it1->second->getPose()->getActiveStateVariables(&states); + // if (states.size() == 0) { + // throw std::runtime_error("Attempted covariance interpolation with locked states"); + // } + + // // TODO(DAVID): Extrapolation behind first entry + + // // Check if we requested before first time + // if (it1 == knotMap_.begin()) { + + // // If we allow extrapolation, return constant-velocity interpolated entry + // if (allowExtrapolation_) { + // const SteamTrajVar::Ptr& startKnot = it1->second; + // TransformEvaluator::Ptr T_t_k = + // ConstVelTransformEvaluator::MakeShared(startKnot->getVelocity(), + // time - startKnot->getTime()); + // return compose(T_t_k, startKnot->getPose()); + // } else { + // throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + // } + // } + + + // return interpCovariance(solver, time); +} + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get active state variables in the trajectory ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/src/trajectory/SteamTrajVar.cpp b/src/trajectory/SteamTrajVar.cpp index 6d7d346..cdf023b 100644 --- a/src/trajectory/SteamTrajVar.cpp +++ b/src/trajectory/SteamTrajVar.cpp @@ -25,6 +25,12 @@ SteamTrajVar::SteamTrajVar(const steam::Time& time, } } +SteamTrajVar::SteamTrajVar(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const Eigen::Matrix cov) + : SteamTrajVar(time, T_k0, velocity) { cov_set_=true; cov_=cov; } + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get pose evaluator ////////////////////////////////////////////////////////////////////////////////////////////// @@ -46,6 +52,14 @@ const VectorSpaceStateVar::Ptr& SteamTrajVar::getAcceleration() const { throw std::runtime_error("Steam trajectory variable does not have an acceleration state!"); } +const Eigen::Matrix SteamTrajVar::getCovariance() const { + return cov_; +} + +bool SteamTrajVar::covarianceSet() const { + return cov_set_; +} + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get timestamp ////////////////////////////////////////////////////////////////////////////////////////////// From 01f7d7cd77faca7b1196ce16e43104f5d51fedd9 Mon Sep 17 00:00:00 2001 From: Jeremy Wong Date: Sat, 17 Oct 2020 18:06:50 -0400 Subject: [PATCH 31/31] dynamic matrix size for covariance --- include/steam/trajectory/SteamTrajVar.hpp | 2 +- src/trajectory/SteamTrajVar.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/steam/trajectory/SteamTrajVar.hpp b/include/steam/trajectory/SteamTrajVar.hpp index e201a6d..7c23076 100644 --- a/include/steam/trajectory/SteamTrajVar.hpp +++ b/include/steam/trajectory/SteamTrajVar.hpp @@ -57,7 +57,7 @@ class SteamTrajVar ////////////////////////////////////////////////////////////////////////////////////////////// const steam::Time& getTime() const; - const Eigen::Matrix getCovariance() const; + const Eigen::MatrixXd getCovariance() const; bool covarianceSet() const; diff --git a/src/trajectory/SteamTrajVar.cpp b/src/trajectory/SteamTrajVar.cpp index cdf023b..6c33c18 100644 --- a/src/trajectory/SteamTrajVar.cpp +++ b/src/trajectory/SteamTrajVar.cpp @@ -52,7 +52,7 @@ const VectorSpaceStateVar::Ptr& SteamTrajVar::getAcceleration() const { throw std::runtime_error("Steam trajectory variable does not have an acceleration state!"); } -const Eigen::Matrix SteamTrajVar::getCovariance() const { +const Eigen::MatrixXd SteamTrajVar::getCovariance() const { return cov_; }