├── .gitignore ├── LICENSE ├── README.md ├── aslam_backend_bsplines_tutorial ├── CMakeLists.txt ├── include │ └── aslam │ │ └── backend │ │ ├── ErrorTermMotionBST.hpp │ │ └── ErrorTermPriorBST.hpp ├── package.xml ├── src │ ├── ErrorTermMotionBST.cpp │ ├── ErrorTermPriorBST.cpp │ └── exampleBST.cpp └── test │ ├── TestErrorTestMotionBST.cpp │ └── test_main.cpp ├── aslam_splines ├── .gitignore ├── CMakeLists.txt ├── include │ └── aslam │ │ ├── backend │ │ ├── BSplineMotionError.hpp │ │ ├── BSplineMotionErrorFactory.hpp │ │ ├── CovarianceReprojectionError.hpp │ │ ├── QuadraticIntegralError.hpp │ │ ├── SimpleSplineError.hpp │ │ └── implementation │ │ │ ├── BSplineMotionError.hpp │ │ │ ├── BSplineMotionErrorFactory.hpp │ │ │ ├── CovarianceReprojectionError.hpp │ │ │ └── SimpleSplineError.hpp │ │ └── splines │ │ ├── BSplineDesignVariable.hpp │ │ ├── BSplineExpressions.hpp │ │ ├── BSplinePoseDesignVariable.hpp │ │ ├── BSplineRSExpressions.hpp │ │ ├── BSplineRSPoseDesignVariable.hpp │ │ ├── EuclideanBSplineDesignVariable.hpp │ │ ├── EuclideanSpline.hpp │ │ ├── OPTBSpline.hpp │ │ ├── OPTEuclideanBSpline.hpp │ │ ├── OPTUnitQuaternionBSpline.hpp │ │ └── implementation │ │ ├── BSplineDesignVariable.hpp │ │ ├── BSplineExpressions.hpp │ │ ├── OPTBSplineImpl.hpp │ │ └── OPTUnitQuaternionBSplineImpl.hpp ├── package.xml ├── src │ ├── BSplineExpressions.cpp │ ├── BSplinePoseDesignVariable.cpp │ ├── BSplineRSExpressions.cpp │ ├── BSplineRSPoseDesignVariable.cpp │ └── EuclideanBSplineDesignVariable.cpp └── test │ ├── TestBSplineExpressions.cpp │ ├── TestErrors.cpp │ ├── TestOPTBSpline.cpp │ └── test_main.cpp ├── aslam_splines_python ├── CMakeLists.txt ├── package.xml ├── python │ └── aslam_splines │ │ └── __init__.py ├── setup.py ├── src │ ├── BSplineMotionError.cpp │ ├── OPTBSpline.cpp │ ├── QuadraticIntegralError.cpp │ ├── SimpleSplineError.cpp │ └── spline_module.cpp └── test │ ├── OptBSpline.py │ └── QuadraticIntegralError.py ├── bsplines ├── .gitignore ├── CMakeLists.txt ├── doc │ ├── doxygen.config.in │ ├── footer.html │ └── header.html ├── include │ └── bsplines │ │ ├── BSpline.hpp │ │ ├── BSplineFitter.hpp │ │ ├── BSplinePose.hpp │ │ ├── DiffManifoldBSpline.hpp │ │ ├── DynamicOrTemplateInt.hpp │ │ ├── EuclideanBSpline.hpp │ │ ├── ExternalIncludes.hpp │ │ ├── KnotArithmetics.hpp │ │ ├── NodeDistributedCache.hpp │ │ ├── NsecTimePolicy.hpp │ │ ├── NumericIntegrator.hpp │ │ ├── SimpleTypeTimePolicy.hpp │ │ ├── UnitQuaternionBSpline.hpp │ │ ├── implementation │ │ ├── BSplineFitterImpl.hpp │ │ ├── DiffManifoldBSplineImpl.hpp │ │ ├── DiffManifoldBSplineTools.hpp │ │ ├── EuclideanBSplineImpl.hpp │ │ └── UnitQuaternionBSplineImpl.hpp │ │ ├── manifolds │ │ ├── DiffManifold.hpp │ │ ├── EuclideanSpace.hpp │ │ ├── LieGroup.hpp │ │ ├── UnitQuaternionManifold.hpp │ │ └── implementation │ │ │ ├── EuclideanSpaceImpl.hpp │ │ │ ├── LieGroupImpl.hpp │ │ │ └── UnitQuaternionManifoldImpl.hpp │ │ └── python │ │ └── DiffManifoldBSplineExporter.hpp ├── mainpage.dox ├── package.xml ├── src │ ├── BSpline.cpp │ ├── BSplinePose.cpp │ └── DiffManifoldBSpline.cpp └── test │ ├── BSplinePoseTests.cpp │ ├── DiffManifoldBSplineTests.cpp │ ├── DiffManifoldBSplineTests.hpp │ ├── EuclideanBSplineTests.cpp │ ├── NodeDistributedCacheTests.cpp │ ├── NumericIntegratorTests.cpp │ ├── RotationalKinematicsTests.cpp │ ├── SplineTests.cpp │ ├── UnitQuaternionBSplineTests.cpp │ └── test_main.cpp ├── bsplines_python ├── .gitignore ├── CMakeLists.txt ├── package.xml ├── python │ └── bsplines │ │ ├── __init__.py │ │ └── plotPoseSpline.py ├── setup.py ├── src │ ├── BSplinePosePython.cpp │ ├── BSplinePython.cpp │ ├── DiffManifoldBSplinePython.cpp │ ├── RotationalKinematicsPython.cpp │ └── SplinePython.cpp └── test │ ├── BSplinePoseTests.py │ ├── BSplineTests.py │ ├── DiffManifoldBSpline.py │ ├── DiffManifoldBSplineFittingExperiments.py │ ├── SplineTests.py │ └── UnitTests.py ├── ci └── prepare-build.sh └── dependencies.rosinstall /.gitignore: -------------------------------------------------------------------------------- 1 | */.settings/org.eclipse.cdt.core.prefs 2 | */.settings/org.eclipse.cdt.ui.prefs 3 | */.settings/org.eclipse.ltk.core.refactoring.prefs 4 | */.settings/language.settings.xml 5 | *# 6 | #* 7 | .#* 8 | *~ 9 | #* 10 | build 11 | *.o 12 | *.so 13 | *.pyc 14 | .project 15 | .cproject 16 | .pydevproject 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, Autonomous Systems Lab, ETH Zurich 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 14 | 3. Neither the name of the Autonomous Systems Lab, ETH Zurich nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | aslam_splines 2 | ============================== 3 | 4 | [![Build Status](http://129.132.38.183:8080/buildStatus/icon?job=aslam_splines)](http://129.132.38.183:8080/job/aslam_splines/) 5 | 6 | B-spline implementations usable as design variables and expression sources for aslam_optimizer (https://github.com/ethz-asl/aslam_optimizer) 7 | 8 | ## License 9 | 3-Clause BSD; see LICENSE 10 | 11 | ## References 12 | The algorithms are based on the following papers. Please cite the appropriate papers when using parts of it in an academic publication. 13 | 14 | 1. Paul Furgale, Joern Rehder, Roland Siegwart (2013). Unified Temporal and Spatial Calibration for Multi-Sensor Systems. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan. 15 | 1. Paul Furgale, T D Barfoot, G Sibley (2012). Continuous-Time Batch Estimation Using Temporal Basis Functions. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 2088–2095, St. Paul, MN. 16 | 1. Hannes Sommer, James R. Forbes, Roland Siegwart, and Paul Furgale. Continuous-Time Estimation of Attitude Using B-Splines on Lie Groups. Journal of Guidance, Control, and Dynamics, 39(2):242–261, February 2016. 17 | 18 | ## Acknowledgments 19 | This work is supported in part by the European Union's Seventh Framework Programme (FP7/2007-2013) under grants #269916 (V-Charge), and #610603 (EUROPA2). 20 | -------------------------------------------------------------------------------- /aslam_backend_bsplines_tutorial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_backend_bsplines_tutorial) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | find_package(Boost REQUIRED COMPONENTS system) 8 | include_directories(${Boost_INCLUDE_DIRS}) 9 | 10 | # enable warnings 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 12 | if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 13 | # deprecated-register warnings are cause by old Eigen versions 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-register") 15 | endif() 16 | 17 | # enable C++11 support 18 | if(CMAKE_VERSION VERSION_LESS "3.1") 19 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 20 | else() 21 | set(CMAKE_CXX_STANDARD 11) 22 | endif() 23 | 24 | cs_add_library(${PROJECT_NAME} 25 | src/ErrorTermMotionBST.cpp 26 | src/ErrorTermPriorBST.cpp 27 | src/exampleBST.cpp 28 | ) 29 | target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) 30 | 31 | 32 | cs_add_executable(${PROJECT_NAME}_exec 33 | src/exampleBST.cpp 34 | ) 35 | target_link_libraries(${PROJECT_NAME}_exec ${PROJECT_NAME}) 36 | 37 | if(CATKIN_ENABLE_TESTING) 38 | catkin_add_gtest(${PROJECT_NAME}_tests 39 | test/test_main.cpp 40 | test/TestErrorTestMotionBST.cpp 41 | ) 42 | if(TARGET ${PROJECT_NAME}_tests) 43 | target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) 44 | endif() 45 | endif() 46 | 47 | cs_install() 48 | cs_export() 49 | -------------------------------------------------------------------------------- /aslam_backend_bsplines_tutorial/include/aslam/backend/ErrorTermMotionBST.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_BACKEND_TUTORIAL_ERROR_MOTION_HPP 2 | #define ASLAM_BACKEND_TUTORIAL_ERROR_MOTION_HPP 3 | 4 | #include 5 | #include 6 | 7 | 8 | namespace aslam { 9 | namespace backend { 10 | 11 | // An error term implementing our observation model. 12 | // This class derives from ErrorTermFs<1> because the 13 | // errors are of dimension 1. 14 | class ErrorTermMotionBST : public ErrorTermFs<1> 15 | { 16 | public: 17 | // This is important. The superclass holds some fixed-sized Eigen types 18 | // For more information, see: 19 | // http://eigen.tuxfamily.org/dox-devel/TopicStructHavingEigenMembers.html 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | 22 | ErrorTermMotionBST(aslam::backend::VectorExpression<1> robotVelocity, double u, double sigma2_u); 23 | virtual ~ErrorTermMotionBST(); 24 | private: 25 | /// This is the inteface required by ErrorTermFs<> 26 | 27 | /// \brief evaluate the error term and return the weighted squared error e^T invR e 28 | virtual double evaluateErrorImplementation(); 29 | 30 | /// \brief evaluate the jacobian 31 | virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians); 32 | 33 | aslam::backend::ScalarExpression _motionErrorTerm; 34 | }; 35 | 36 | } // namespace backend 37 | } // namespace aslam 38 | 39 | 40 | #endif /* ASLAM_BACKEND_TUTORIAL_ERROR_MOTION_HPP */ 41 | -------------------------------------------------------------------------------- /aslam_backend_bsplines_tutorial/include/aslam/backend/ErrorTermPriorBST.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_BACKEND_TUTORIAL_ERROR_PRIOR_HPP 2 | #define ASLAM_BACKEND_TUTORIAL_ERROR_PRIOR_HPP 3 | 4 | #include 5 | #include 6 | 7 | 8 | namespace aslam { 9 | namespace backend { 10 | class ErrorTermPriorBST : public ErrorTermFs<1> 11 | { 12 | public: 13 | // This is important. The superclass holds some fixed-sized Eigen types 14 | // For more information, see: 15 | // http://eigen.tuxfamily.org/dox-devel/TopicStructHavingEigenMembers.html 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | ErrorTermPriorBST(aslam::backend::VectorExpression<1> robotPos, double hat_x, double sigma2_x); 19 | virtual ~ErrorTermPriorBST(); 20 | 21 | private: 22 | /// This is the inteface required by ErrorTermFs<> 23 | 24 | /// \brief evaluate the error term and return the weighted squared error e^T invR e 25 | virtual double evaluateErrorImplementation(); 26 | 27 | /// \brief evaluate the jacobian 28 | virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians); 29 | 30 | aslam::backend::VectorExpression<1> _robotPos; 31 | double _hat_x; 32 | 33 | }; 34 | } // namespace backend 35 | } // namespace aslam 36 | 37 | 38 | #endif /* ASLAM_BACKEND_TUTORIAL_ERROR_PRIOR_HPP */ 39 | -------------------------------------------------------------------------------- /aslam_backend_bsplines_tutorial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_backend_bsplines_tutorial 4 | 0.0.1 5 | aslam_backend_bsplines_tutorial 6 | Mathias Buerki 7 | Mathias Buerki 8 | BSD-3-Clause 9 | catkin 10 | catkin_simple 11 | aslam_backend 12 | aslam_backend_expressions 13 | aslam_splines 14 | bsplines 15 | sm_kinematics 16 | 17 | aslam_backend 18 | aslam_backend_expressions 19 | aslam_splines 20 | bsplines 21 | sm_kinematics 22 | 23 | 24 | -------------------------------------------------------------------------------- /aslam_backend_bsplines_tutorial/src/ErrorTermMotionBST.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace aslam { 4 | namespace backend { 5 | 6 | ErrorTermMotionBST::ErrorTermMotionBST(aslam::backend::VectorExpression<1> robotVelocity, double u, double sigma2_u) : 7 | _motionErrorTerm(ScalarExpression(boost::shared_ptr(new ScalarExpressionNodeConstant(u))) - robotVelocity.toScalarExpression()) 8 | { 9 | 10 | Eigen::Matrix invR; 11 | invR(0,0) = (1.0/sigma2_u); 12 | // Fill in the inverse covariance. In this scalar case, this is just an inverse variance. 13 | setInvR( invR ); 14 | 15 | // Tell the super class about the design variables: 16 | DesignVariable::set_t dvs; 17 | robotVelocity.getDesignVariables(dvs); 18 | ErrorTermFs<1>::setDesignVariablesIterator(dvs.begin(), dvs.end()); 19 | } 20 | 21 | ErrorTermMotionBST::~ErrorTermMotionBST() 22 | { 23 | 24 | } 25 | 26 | 27 | /// \brief evaluate the error term and return the weighted squared error e^T invR e 28 | double ErrorTermMotionBST::evaluateErrorImplementation() 29 | { 30 | error_t error; 31 | error(0) = _motionErrorTerm.toScalar(); 32 | //std::cout << "The motion error is: " << error(0) << std::endl; 33 | setError(error); 34 | return evaluateChiSquaredError(); 35 | } 36 | 37 | 38 | /// \brief evaluate the jacobian 39 | void ErrorTermMotionBST::evaluateJacobiansImplementation(JacobianContainer & outJacobians) 40 | { 41 | _motionErrorTerm.evaluateJacobians(outJacobians); 42 | } 43 | 44 | } // namespace backend 45 | } // namespace aslam 46 | -------------------------------------------------------------------------------- /aslam_backend_bsplines_tutorial/src/ErrorTermPriorBST.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace aslam { 4 | namespace backend { 5 | ErrorTermPriorBST::ErrorTermPriorBST(aslam::backend::VectorExpression<1> robotPos, double hat_x, double sigma2_x) 6 | : _robotPos(robotPos), _hat_x(hat_x) 7 | { 8 | // Fill in the inverse covariance. In this scalar case, this is just an inverse variance. 9 | Eigen::Matrix invR = Eigen::Matrix::Identity(); 10 | invR(0,0) = ( 1.0/sigma2_x); 11 | setInvR( invR ); 12 | 13 | // Tell the super class about the design variables: 14 | DesignVariable::set_t dvs; 15 | robotPos.getDesignVariables(dvs); 16 | ErrorTermFs<1>::setDesignVariablesIterator(dvs.begin(), dvs.end()); 17 | 18 | } 19 | 20 | ErrorTermPriorBST::~ErrorTermPriorBST() 21 | { 22 | 23 | } 24 | 25 | 26 | /// \brief evaluate the error term and return the weighted squared error e^T invR e 27 | double ErrorTermPriorBST::evaluateErrorImplementation() 28 | { 29 | 30 | double robotPos = _robotPos.evaluate()(0); 31 | 32 | Eigen::VectorXd err = Eigen::VectorXd(1); 33 | err(0) = robotPos - _hat_x; 34 | //std::cout << "err: " << err << std::endl; 35 | setError(err); 36 | return evaluateChiSquaredError(); 37 | } 38 | 39 | 40 | /// \brief evaluate the jacobian 41 | void ErrorTermPriorBST::evaluateJacobiansImplementation(JacobianContainer & outJacobians) 42 | { 43 | _robotPos.evaluateJacobians(outJacobians); 44 | } 45 | 46 | 47 | } // namespace backend 48 | } // namespace aslam 49 | -------------------------------------------------------------------------------- /aslam_backend_bsplines_tutorial/src/exampleBST.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | // Bring in some random number generation from Schweizer Messer. 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | if(argc != 2) 24 | { 25 | std::cout << "Usage: example K\n"; 26 | std::cout << "The argument K is the number of timesteps to include in the optimization\n"; 27 | return 1; 28 | } 29 | 30 | const int K = atoi(argv[1]); 31 | 32 | try 33 | { 34 | // The true wall position 35 | const double true_w = 5.0; 36 | 37 | // The noise properties. 38 | const double sigma_n = 0.01; 39 | const double sigma_u = 0.1; 40 | const double sigma_x = 0.01; 41 | 42 | // Create random odometry 43 | std::vector true_u_k(K); 44 | for(double& u : true_u_k) 45 | { 46 | u = 1;//sm::random::uniform(); 47 | } 48 | 49 | // Create the noisy odometry 50 | std::vector u_k(K); 51 | for(int k = 0; k < K; ++k) 52 | { 53 | u_k[k] = true_u_k[k] + (sigma_u * sm::random::normal()); 54 | } 55 | 56 | // Create the states from noisy odometry. 57 | std::vector x_k(K); 58 | std::vector true_x_k(K); 59 | x_k[0] = 10.0; 60 | true_x_k[0] = 10.0; 61 | for(int k = 1; k < K; ++k) 62 | { 63 | true_x_k[k] = true_x_k[k-1] + true_u_k[k]; 64 | x_k[k] = x_k[k-1] + u_k[k]; 65 | } 66 | 67 | 68 | // Create the noisy measurments 69 | std::vector y_k(K); 70 | for(int k = 0; k < K; ++k) 71 | { 72 | y_k[k] = (true_w / true_x_k[k]) + sigma_n * sm::random::normal(); 73 | } 74 | 75 | // Now we can build an optimization problem. 76 | boost::shared_ptr problem( new aslam::backend::OptimizationProblem); 77 | 78 | typedef aslam::splines::OPTBSpline::CONF> PosSpline; 79 | PosSpline robotPosSpline; 80 | const int pointSize = robotPosSpline.getPointSize(); 81 | 82 | PosSpline::point_t initPoint(pointSize); 83 | 84 | initPoint(0,0) = x_k[0]; 85 | 86 | // initialize the spline uniformly with from time 0 to K with K segments and the initPoint as a constant value 87 | robotPosSpline.initConstantUniformSpline(0, K, K, initPoint); 88 | 89 | // add the robot pose spline to the problem 90 | for(size_t i = 0; i < robotPosSpline.numDesignVariables(); i++) 91 | { 92 | robotPosSpline.designVariable(i)->setActive(true); 93 | problem->addDesignVariable(robotPosSpline.designVariable(i), false); 94 | } 95 | 96 | // set up wall position 97 | double wallPosition = true_w + sm::random::normal(); 98 | std::cout << "Noisy wall position : " << wallPosition << std::endl; 99 | 100 | // First, create a design variable for the wall position. 101 | boost::shared_ptr dv_w(new aslam::backend::Scalar(true_w)); 102 | // Setting this active means we estimate it. 103 | dv_w->setActive(true); 104 | // Add it to the optimization problem. 105 | problem->addDesignVariable(dv_w.get(), false); 106 | 107 | // Now create a prior for this initial state. 108 | auto vecPosExpr = robotPosSpline.getExpressionFactoryAt<0>(0).getValueExpression(0); 109 | boost::shared_ptr prior(new aslam::backend::ErrorTermPriorBST(vecPosExpr, true_x_k[0], sigma_x * sigma_x)); 110 | // and add it to the problem. 111 | problem->addErrorTerm(prior); 112 | 113 | // Now march through the states creating design variables, 114 | // odometry error terms and measurement error terms. 115 | for(int k = 0; k < K; ++k) 116 | { 117 | // Create expression factory at time k, prepared for time derivatives up to 1 (for that we need the "<1>"). 118 | auto exprFactory = robotPosSpline.getExpressionFactoryAt<1>(k); 119 | 120 | // Create odometry error via ErrorTermMotionBST 121 | auto vecVelExpr = exprFactory.getValueExpression(1); // 1 => first derivative of position ~ robot velocity 122 | boost::shared_ptr em(new aslam::backend::ErrorTermMotionBST(vecVelExpr, u_k[k], sigma_u * sigma_u)); 123 | problem->addErrorTerm(em); 124 | 125 | // Create observation error using expressions and toErrorTerm to create an ExpressionErrorTerm 126 | auto vecPosExpr = exprFactory.getValueExpression(0); // 0 => spline value itself ~ robot position 127 | // We want to compute an error term e := dv_w / vecPosExpr[0] - y_k[k] + v, with v ~ N(0, sigma_n) 128 | problem->addErrorTerm(toErrorTerm(dv_w->toExpression() / vecPosExpr.toScalarExpression<0>() - y_k[k], 1.0 / (sigma_n * sigma_n))); 129 | } 130 | 131 | // Now we have a valid optimization problem full of design variables and error terms. 132 | // Create some optimization options. 133 | aslam::backend::Optimizer2Options options; 134 | options.verbose = true; 135 | options.linearSystemSolver.reset(new aslam::backend::DenseQrLinearSystemSolver()); 136 | // options.levenbergMarquardtLambdaInit = 10; 137 | options.doSchurComplement = false; 138 | // options.doLevenbergMarquardt = true; 139 | // Force it to over-optimize 140 | options.convergenceDeltaX = 1e-12; 141 | options.convergenceDeltaError = 1e-12; 142 | // Then create the optimizer and go! 143 | aslam::backend::Optimizer2 optimizer(options); 144 | optimizer.setProblem(problem); 145 | 146 | optimizer.optimize(); 147 | 148 | // the wall is at 149 | std::cout << "After optimization, the wall is at: " << std::endl << dv_w->toExpression().toScalar() << std::endl; 150 | 151 | for(int i = 0; i < K; i++) 152 | { 153 | // This time we don't need expressions because we are only going to print the values. There fore we only create an "evaluator" instead of an expression factory. 154 | // Create evaluator at time k, supporting time derivatives up to 1 (for that we need the "<1>"). 155 | auto evaluator = robotPosSpline.getEvaluatorAt<1>(i); 156 | auto vecPos = evaluator.evalD(0); 157 | auto vecVel = evaluator.evalD(1); 158 | 159 | std::cout << "Robot at " << i << " is: " << vecPos(0) << std::endl; 160 | std::cout << "Velocity at " << i << " is: " << vecVel(0) << std::endl; 161 | } 162 | } 163 | catch(const std::exception & e) 164 | { 165 | std::cout << "Exception during processing: " << e.what(); 166 | return 1; 167 | } 168 | 169 | std::cout << "Processing completed successfully\n"; 170 | return 0; 171 | } 172 | -------------------------------------------------------------------------------- /aslam_backend_bsplines_tutorial/test/TestErrorTestMotionBST.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | // This test harness makes it easy to test error terms. 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | template struct ConfCreator { 19 | static inline TConf create(){ 20 | return TConf(typename TConf::ManifoldConf(IDim), ISplineOrder); 21 | } 22 | }; 23 | 24 | template struct ConfCreator { 25 | static inline TConf create(){ 26 | BOOST_STATIC_ASSERT_MSG(IDim == TConf::Dimension::VALUE, "impossible dimension selected!"); 27 | return TConf(typename TConf::ManifoldConf(), ISplineOrder); 28 | } 29 | }; 30 | 31 | template inline TConf createConf(){ 32 | return ConfCreator::create(); 33 | } 34 | 35 | 36 | TEST(AslamVChargeBackendTestSuite, testEuclidean) 37 | { 38 | try { 39 | using namespace aslam::backend; 40 | 41 | double sigma_n = 0.5; 42 | 43 | typedef aslam::splines::OPTBSpline::CONF> PosSpline; 44 | PosSpline robotPosSpline; 45 | const int pointSize = robotPosSpline.getPointSize(); 46 | 47 | PosSpline::point_t initPoint(pointSize); 48 | initPoint(0,0) = 10.0; 49 | 50 | robotPosSpline.initConstantUniformSpline(0, 10, 10, initPoint); 51 | 52 | // First, create a design variable for the wall position. 53 | boost::shared_ptr dv_w(new aslam::backend::Scalar(5.0)); 54 | 55 | // Create observation error 56 | auto vecVelExpr = robotPosSpline.getExpressionFactoryAt<1>(5).getValueExpression(1); 57 | 58 | aslam::backend::ErrorTermMotionBST eo(vecVelExpr, 1.0, sigma_n * sigma_n); 59 | 60 | EXPECT_NEAR(1.0/(sigma_n * sigma_n), eo.evaluateError(), 1e-14); 61 | 62 | // Create the test harness 63 | aslam::backend::ErrorTermTestHarness<1> harness(&eo); 64 | 65 | // Run the unit tests. 66 | harness.testAll(1e-5); 67 | } 68 | catch(const std::exception & e) { 69 | FAIL() << e.what(); 70 | } 71 | } 72 | -------------------------------------------------------------------------------- /aslam_backend_bsplines_tutorial/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | 9 | -------------------------------------------------------------------------------- /aslam_splines/.gitignore: -------------------------------------------------------------------------------- 1 | python/aslam_splines/libaslam_splines_python.so 2 | /LocalBuildType.cmake 3 | -------------------------------------------------------------------------------- /aslam_splines/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_splines) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | find_package(Boost REQUIRED COMPONENTS system) 8 | include_directories(${Boost_INCLUDE_DIRS}) 9 | 10 | # enable warnings 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 12 | if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 13 | # deprecated-register warnings are cause by old Eigen versions 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-register") 15 | endif() 16 | 17 | # enable C++11 support 18 | if(CMAKE_VERSION VERSION_LESS "3.1") 19 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 20 | else() 21 | set(CMAKE_CXX_STANDARD 11) 22 | endif() 23 | 24 | cs_add_library(${PROJECT_NAME} 25 | src/BSplinePoseDesignVariable.cpp 26 | src/BSplineRSPoseDesignVariable.cpp 27 | src/BSplineExpressions.cpp 28 | src/BSplineRSExpressions.cpp 29 | src/EuclideanBSplineDesignVariable.cpp 30 | ) 31 | target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) 32 | 33 | if(CATKIN_ENABLE_TESTING) 34 | catkin_add_gtest(${PROJECT_NAME}_tests 35 | test/test_main.cpp 36 | test/TestBSplineExpressions.cpp 37 | test/TestOPTBSpline.cpp 38 | test/TestErrors.cpp 39 | ) 40 | if(TARGET ${PROJECT_NAME}_tests) 41 | target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) 42 | endif() 43 | endif() 44 | 45 | cs_install() 46 | cs_export() 47 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/backend/BSplineMotionError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_BACKEND_MOTION_ERROR_HPP 2 | #define ASLAM_BACKEND_MOTION_ERROR_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace aslam { 10 | namespace backend { 11 | 12 | // An error term implementing our observation model. 13 | // This class derives from ErrorTermFs<1> because the 14 | // errors are of dimension 1. 15 | template 16 | class BSplineMotionError : public ErrorTermFs<1> 17 | { 18 | public: 19 | // This is important. The superclass holds some fixed-sized Eigen types 20 | // For more information, see: 21 | // http://eigen.tuxfamily.org/dox-devel/TopicStructHavingEigenMembers.html 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | // typedef aslam::splines::BSplinePoseDesignVariable spline_t; 25 | typedef SPLINE_T spline_t; 26 | typedef sparse_block_matrix::SparseBlockMatrix sbm_t; 27 | 28 | 29 | // BSplineMotionError(ScalarDesignVariable * x_k, ScalarDesignVariable * w, double y, double sigma2_n); 30 | BSplineMotionError(spline_t * splineDV, Eigen::MatrixXd W); 31 | BSplineMotionError(spline_t * splineDV, Eigen::MatrixXd W, unsigned int errorTermOrder); 32 | 33 | virtual ~BSplineMotionError(); 34 | 35 | 36 | // usefull for debugging / error checking 37 | Eigen::MatrixXd Q() { return _Q.toDense(); }; 38 | Eigen::VectorXd rhs(); 39 | 40 | 41 | protected: 42 | /// This is the inteface required by ErrorTermFs<> 43 | 44 | /// \brief evaluate the error term and return the weighted squared error e^T invR e 45 | virtual double evaluateErrorImplementation(); 46 | 47 | /// \brief evaluate the jacobian 48 | virtual void evaluateJacobiansImplementation(aslam::backend::JacobianContainer & J); 49 | 50 | virtual void buildHessianImplementation(SparseBlockMatrix & outHessian, Eigen::VectorXd & outRhs,bool useMEstimator); 51 | 52 | private: 53 | // ScalarDesignVariable * _x_k; 54 | // ScalarDesignVariable * _w; 55 | spline_t * _splineDV; 56 | Eigen::MatrixXd _W; 57 | sbm_t _Q; 58 | unsigned int _coefficientVectorLength; 59 | 60 | void initialize(spline_t * splineDV, Eigen::MatrixXd W, unsigned int errorTermOrder); 61 | 62 | }; 63 | 64 | } // namespace backend 65 | } // namespace aslam 66 | 67 | #include "implementation/BSplineMotionError.hpp" 68 | 69 | 70 | #endif /* ASLAM_BACKEND_MOTION_ERROR_HPP */ 71 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/backend/BSplineMotionErrorFactory.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _BSPLINEMOTIONERRORFACTORY_H_ 2 | #define _BSPLINEMOTIONERRORFACTORY_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | namespace aslam { 12 | namespace backend { 13 | 14 | template 15 | void addMotionErrorTerms(OptimizationProblemBase& problem, BSplineDesignVariable& spline, const Eigen::MatrixXd& W, unsigned int errorTermOrder); 16 | 17 | } // namespace backend 18 | } // namespace aslam 19 | 20 | #include "implementation/BSplineMotionErrorFactory.hpp" 21 | 22 | #endif /* _BSPLINEMOTIONERRORFACTORY_H_ */ 23 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/backend/CovarianceReprojectionError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_BACKEND_COVARIANCE_REPROJECTION_ERROR_HPP 2 | #define ASLAM_BACKEND_COVARIANCE_REPROJECTION_ERROR_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace aslam { 14 | namespace backend { 15 | 16 | template 17 | class CovarianceReprojectionError : public ErrorTermFs< FRAME_T::KeypointDimension > 18 | { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | SM_DEFINE_EXCEPTION(Exception, std::runtime_error); 22 | 23 | 24 | typedef FRAME_T frame_t; 25 | typedef typename frame_t::keypoint_t keypoint_t; 26 | typedef typename frame_t::camera_geometry_t camera_geometry_t; 27 | typedef aslam::splines::BSplinePoseDesignVariable spline_t; 28 | 29 | enum { 30 | KeypointDimension = frame_t::KeypointDimension /*!< The dimension of the keypoint associated with this geometry policy */ 31 | }; 32 | 33 | typedef Eigen::Matrix measurement_t; 34 | typedef Eigen::Matrix inverse_covariance_t; 35 | typedef ErrorTermFs< KeypointDimension > parent_t; 36 | 37 | CovarianceReprojectionError(); 38 | // we take the lineDelayDv scalar design variable as an additional parameter as it was 39 | // the structure in the first place and the Jacobians of the Shutters are not yet 40 | // implemented. This should be removed at some point, to only keep the camera 41 | // design variable. 42 | // if the lineDelayDv is initialised to NULL, the camera design variable will be used. 43 | CovarianceReprojectionError(const frame_t * frame, int keypointIndex, HomogeneousExpression point, CameraDesignVariable camera, spline_t* spline = NULL, Scalar * lineDelayDv = NULL); 44 | virtual ~CovarianceReprojectionError(); 45 | 46 | double observationTime(); 47 | 48 | Eigen::MatrixXd covarianceMatrix(); 49 | 50 | protected: 51 | /// \brief evaluate the error term 52 | virtual double evaluateErrorImplementation(); 53 | 54 | /// \brief evaluate the jacobian 55 | virtual void evaluateJacobiansImplementation(JacobianContainer & J); 56 | 57 | /// \brief the frame that this measurement comes from. 58 | const frame_t * _frame; 59 | 60 | /// \brief the keypoint index within the frame. 61 | int _keypointIndex; 62 | 63 | /// \brief the homogeneous point expressed in the camera frame 64 | HomogeneousExpression _point; 65 | 66 | CameraDesignVariable _camera; 67 | 68 | spline_t * _spline; 69 | Scalar * _lineDelayDv; 70 | }; 71 | } // namespace backend 72 | } // namespace aslam 73 | 74 | #include "implementation/CovarianceReprojectionError.hpp" 75 | 76 | #endif /* ASLAM_BACKEND_COVARIANCE_REPROJECTION_ERROR_HPP */ 77 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/backend/QuadraticIntegralError.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * IntegrationErrorTerm.hpp 3 | * 4 | * Created on: Jan 23, 2013 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef QINTEGRALERRORTERM_HPP_ 9 | #define QINTEGRALERRORTERM_HPP_ 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | namespace aslam { 29 | namespace backend { 30 | namespace integration { 31 | 32 | namespace algorithms = ::numeric_integrator::algorithms; 33 | typedef algorithms::Default DefaultAlgorithm; 34 | 35 | namespace internal { 36 | template 37 | inline void addErrorTermToProblem(ErrorTermReceiver & problem, ErrorTerm *et, bool problemOwnsErrorTerms){ 38 | problem.addErrorTerm(problemOwnsErrorTerms ? boost::shared_ptr(et) : boost::shared_ptr(et, sm::null_deleter())); 39 | } 40 | template 41 | inline void addErrorTermToProblem(ErrorTermReceiver & problem, boost::shared_ptr et, bool /* problemOwnsErrorTerms */){ 42 | problem.addErrorTerm(et); 43 | } 44 | } 45 | 46 | 47 | /** 48 | To add an error term of the form 49 | 50 | e = Int E(t)^T W(t) E(t) dt 51 | 52 | to a problem use this function when E(t) is implemented as an ErrorTerm. If E is an expression use the next function. 53 | 54 | The ErrorTermFactory functor must have an 55 | 56 | ErrorTerm* operator()(TTime t, double factor) const 57 | or 58 | boost::shared_ptr operator()(TTime t, double factor) const 59 | 60 | member that returns E(t) with its invR = W * factor. 61 | So don't forget to take the square root of factor when you use the setSqrtInvR method. 62 | 63 | When a shared_ptr is returned the problemOwnsErrorTerms flag is ignored. Otherwise true makes the problem deleting the error terms in its destructor. 64 | */ 65 | template 66 | void addQuadraticIntegralErrorTerms(ErrorTermReceiver & problem, const TTime & a, const TTime & b, int numberOfPoints, const ErrorTermFactory & errorTermFactory, bool problemOwnsErrorTerms = true) 67 | { 68 | if(a == b) return; 69 | 70 | auto integrator = Algorithm().template getIntegrator(a, b, numberOfPoints); 71 | SM_ASSERT_TRUE(std::runtime_error, !integrator.isAtEnd(), "too few integration points given : " << numberOfPoints); 72 | 73 | const double commonFactor = integrator.getCommonFactor(); 74 | for(; !integrator.isAtEnd(); integrator.next()){ 75 | double valueFactor = integrator.getValueFactor(); 76 | internal::addErrorTermToProblem(problem, errorTermFactory(integrator.getIntegrationScalar(), commonFactor * valueFactor), problemOwnsErrorTerms); 77 | } 78 | } 79 | 80 | template 81 | void addQuadraticIntegralErrorTerms(ErrorTermReceiver & problem, const TTime & a, const TTime & b, int numberOfPoints, const ErrorTermFactory & errorTermFactory, bool problemOwnsErrorTerms = true){ 82 | addQuadraticIntegralErrorTerms(problem, a, b, numberOfPoints, errorTermFactory, problemOwnsErrorTerms); 83 | } 84 | 85 | /** 86 | To add an error term of the form 87 | 88 | e = \Int E(t)^T sqrtInvR^T sqrtInvR E(t) dt 89 | 90 | to a problem use this function when E(t) is implemented as an VectorExpression. 91 | 92 | The ExpressionFactory functor must have an 93 | 94 | VectorExpression operator()(TTime t) const 95 | or 96 | GenericMatrixExpression operator()(TTime t) const 97 | 98 | member that returns E(t). 99 | */ 100 | template 101 | void addQuadraticIntegralExpressionErrorTerms(ErrorTermReceiver & problem, const TTime & a, const TTime & b, int numberOfPoints, const ExpressionFactory & expressionFactory, const Eigen::MatrixBase & sqrtInvR) 102 | { 103 | addQuadraticIntegralErrorTerms( 104 | problem, a, b, numberOfPoints, 105 | [&expressionFactory, &sqrtInvR](TTime t, double f){ 106 | return toErrorTermSqrt(expressionFactory(t), sqrtInvR * sqrt(f)); 107 | } 108 | ); 109 | } 110 | 111 | // to specify a default for the algorithm, 112 | template 113 | void addQuadraticIntegralExpressionErrorTerms(ErrorTermReceiver & problem, const TTime & a, const TTime & b, int numberOfPoints, const ExpressionFactory & expressionFactory, const Eigen::MatrixBase & sqrtInvR){ 114 | addQuadraticIntegralExpressionErrorTerms(problem, a, b, numberOfPoints, expressionFactory, sqrtInvR); 115 | } 116 | 117 | 118 | } 119 | } 120 | } 121 | 122 | #endif /* QINTEGRALERRORTERM_HPP_ */ 123 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/backend/SimpleSplineError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_BACKEND_SPLINE_ERROR_HPP 2 | #define ASLAM_BACKEND_SPLINE_ERROR_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace aslam { 10 | namespace backend { 11 | 12 | // An error term implementing our observation model. 13 | // This class derives from ErrorTermFs<1> because the 14 | // errors are of dimension 1. 15 | template 16 | class SimpleSplineError : public ErrorTermFs<1> 17 | { 18 | public: 19 | // This is important. The superclass holds some fixed-sized Eigen types 20 | // For more information, see: 21 | // http://eigen.tuxfamily.org/dox-devel/TopicStructHavingEigenMembers.html 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | // typedef aslam::splines::BSplinePoseDesignVariable spline_t; 25 | typedef SPLINE_T spline_t; 26 | typedef sparse_block_matrix::SparseBlockMatrix sbm_t; 27 | typedef ErrorTermFs< 1 > parent_t; 28 | typedef aslam::backend::VectorExpression expression_t; 29 | 30 | 31 | SimpleSplineError(spline_t * splineDV, expression_t* splineExpression, Eigen::Matrix y, double t); 32 | 33 | virtual ~SimpleSplineError(); 34 | 35 | 36 | protected: 37 | /// This is the inteface required by ErrorTermFs<> 38 | 39 | /// \brief evaluate the error term and return the weighted squared error e^T invR e 40 | virtual double evaluateErrorImplementation(); 41 | 42 | /// \brief evaluate the jacobian 43 | virtual void evaluateJacobiansImplementation(aslam::backend::JacobianContainer & J); 44 | 45 | // virtual void buildHessianImplementation(SparseBlockMatrix & outHessian, Eigen::VectorXd & outRhs,bool useMEstimator); 46 | 47 | // virtual const JacobianContainer & getJacobiansImplementation() const; 48 | 49 | private: 50 | 51 | spline_t * _splineDV; 52 | // boost::shared_ptr< expression_t > _splineExpression; 53 | expression_t* _splineExpression; 54 | 55 | Eigen::VectorXd _y; 56 | double _t; 57 | 58 | }; 59 | 60 | } // namespace backend 61 | } // namespace aslam 62 | 63 | #include "implementation/SimpleSplineError.hpp" 64 | 65 | 66 | #endif /* ASLAM_BACKEND_SPLINE_ERROR_HPP */ 67 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/backend/implementation/BSplineMotionError.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | namespace aslam { 4 | namespace backend { 5 | 6 | 7 | 8 | template 9 | BSplineMotionError::BSplineMotionError(spline_t * splineDV, Eigen::MatrixXd W): 10 | _splineDV(splineDV), _W(W) 11 | { 12 | initialize(splineDV, W, 2); // default: acceleration criterion 13 | } 14 | 15 | template 16 | BSplineMotionError::BSplineMotionError(spline_t * splineDV, Eigen::MatrixXd W, unsigned int errorTermOrder): 17 | _splineDV(splineDV), _W(W) 18 | { 19 | initialize(splineDV, W, errorTermOrder); 20 | } 21 | 22 | template 23 | BSplineMotionError::~BSplineMotionError() 24 | { 25 | 26 | } 27 | 28 | template 29 | void BSplineMotionError::initialize(spline_t * /* splineDV */, Eigen::MatrixXd /* W */, unsigned int errorTermOrder) { 30 | 31 | // check spline order: 32 | int splineOrder = _splineDV->spline().splineOrder(); 33 | if(splineOrder <= (int)errorTermOrder && splineOrder >= 2) { 34 | errorTermOrder = splineOrder-1; 35 | std::cout << "! Invalid ErrorTermOrder reduced to " << errorTermOrder << std::endl; 36 | } 37 | sbm_t Qsp = _splineDV->spline().curveQuadraticIntegralSparse(_W, errorTermOrder); 38 | 39 | // set spline design variables 40 | unsigned int numberOfSplineDesignVariables = _splineDV->spline().numVvCoefficients(); 41 | _coefficientVectorLength = _splineDV->spline().coefficients().rows() * numberOfSplineDesignVariables; 42 | 43 | Qsp.cloneInto(_Q); 44 | // std::cout << "next:" << std::endl; 45 | 46 | // std::cout << Q.cols() << ":" << Q.rows() << std::endl; 47 | // std::cout << _Q->cols() << ":" << _Q->rows() << std::endl; 48 | 49 | // Tell the super class about the design variables: 50 | // loop the design variables and add to vector: 51 | std::vector dvV; 52 | for ( unsigned int i = 0; i < numberOfSplineDesignVariables; i++) { 53 | dvV.push_back(_splineDV->designVariable(i)); 54 | } 55 | setDesignVariables(dvV); 56 | 57 | } 58 | 59 | 60 | /// \brief evaluate the error term and return the weighted squared error e^T invR e 61 | template 62 | double BSplineMotionError::evaluateErrorImplementation() 63 | { 64 | // the error is a scalar: c' Q c, with c the vector valued spline coefficients stacked 65 | 66 | const double* cMat = &((_splineDV->spline()).coefficients()(0,0)); 67 | Eigen::Map c = Eigen::VectorXd::Map(cMat, _coefficientVectorLength); 68 | 69 | // Q*c : 70 | // create result container: 71 | Eigen::VectorXd Qc(_Q.rows()); // number of rows of Q: 72 | Qc.setZero(); 73 | // std::cout << Qc->rows() << ":" << Qc->cols() << std::endl; 74 | _Q.multiply(&Qc, c); 75 | 76 | return c.transpose() * (Qc); 77 | } 78 | 79 | 80 | /// \brief evaluate the jacobians 81 | template 82 | void BSplineMotionError::evaluateJacobiansImplementation(aslam::backend::JacobianContainer & /* _jacobians */) 83 | { 84 | 85 | // this is an error... 86 | SM_THROW(Exception, "This is currently unsupported"); 87 | 88 | } 89 | 90 | 91 | 92 | template 93 | void BSplineMotionError::buildHessianImplementation(SparseBlockMatrix & outHessian, Eigen::VectorXd & outRhs, bool /* useMEstimator */) { 94 | 95 | 96 | // get the coefficients: 97 | Eigen::MatrixXd coeff = _splineDV->spline().coefficients(); 98 | // create a column vector of spline coefficients 99 | int dim = coeff.rows(); 100 | int seg = coeff.cols(); 101 | // build a vector of coefficients: 102 | Eigen::VectorXd c(dim*seg); 103 | // rows are spline dimension 104 | for(int i = 0; i < seg; i++) { 105 | c.block(i*dim,0,dim,1) = coeff.block(0, i, dim,1); 106 | } 107 | 108 | // right hand side: 109 | Eigen::VectorXd b_u(_Q.rows()); // number of rows of Q: 110 | 111 | b_u.setZero(); 112 | /* std::cout <<"b" << std::endl; 113 | for(int i = 0 ; i < b_u->rows(); i++) 114 | std::cout << (*b_u)(i) << std::endl; 115 | std::cout <<"/b" << std::endl; */ 116 | 117 | _Q.multiply(&b_u, c); 118 | 119 | // place the hessian elements in the correct place: 120 | 121 | // build hessian: 122 | for(size_t i = 0; i < numDesignVariables(); i++) 123 | { 124 | 125 | if( designVariable(i)->isActive()) { 126 | 127 | // get the block index 128 | int colBlockIndex = designVariable(i)->blockIndex(); 129 | int rows = designVariable(i)->minimalDimensions(); 130 | int rowBase = outHessian.colBaseOfBlock(colBlockIndex); 131 | 132 | // <- this is our column index 133 | //_numberOfSplineDesignVariables 134 | for(size_t j = 0; j <= i; j++) // upper triangle should be sufficient 135 | { 136 | 137 | if (designVariable(j)->isActive()) { 138 | 139 | int rowBlockIndex = designVariable(j)->blockIndex(); 140 | 141 | // select the corresponding block in _Q: 142 | Eigen::MatrixXd* Qblock = _Q.block(i,j, false); // get block and do NOT allocate. 143 | 144 | if (Qblock) { // check if block exists 145 | // get the Hessian Block 146 | const bool allocateIfMissing = true; 147 | Eigen::MatrixXd *Hblock = outHessian.block(rowBlockIndex, colBlockIndex, allocateIfMissing); 148 | *Hblock += *Qblock; // insert! 149 | } 150 | } 151 | 152 | } 153 | 154 | outRhs.segment(rowBase, rows) -= b_u.segment(i*rows, rows); 155 | } 156 | } 157 | 158 | 159 | //std::cout << "OutHessian" << outHessian.toDense() << std::endl; 160 | 161 | // show outRhs: 162 | // for (int i = 0; i < outRhs.rows(); i++) 163 | // std::cout << outRhs(i) << " : " << (*b_u)(i) << std::endl; 164 | 165 | 166 | } 167 | 168 | 169 | template 170 | Eigen::VectorXd BSplineMotionError::rhs() { 171 | 172 | const double* cMat = &((_splineDV->spline()).coefficients()(0,0)); 173 | Eigen::Map c = Eigen::VectorXd::Map(cMat, _coefficientVectorLength); 174 | 175 | // right hand side: 176 | Eigen::VectorXd b_u(_Q.rows()); // number of rows of Q: 177 | b_u.setZero(); 178 | _Q.multiply(&b_u, -c); 179 | 180 | return b_u; 181 | } 182 | 183 | 184 | 185 | } // namespace backend 186 | } // namespace aslam 187 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/backend/implementation/BSplineMotionErrorFactory.hpp: -------------------------------------------------------------------------------- 1 | 2 | namespace aslam { 3 | namespace backend { 4 | 5 | template 6 | void addMotionErrorTerms(OptimizationProblemBase& problem, SplineDv& splineDv, const Eigen::MatrixXd& W, unsigned int errorTermOrder) { 7 | // Add one error term for each segment 8 | auto spline = splineDv.spline(); 9 | 10 | for(int i = 0; i < spline.numValidTimeSegments(); ++i) { 11 | Eigen::MatrixXd R = spline.segmentIntegral(i, W, errorTermOrder); 12 | Eigen::VectorXd c = spline.segmentCoefficientVector(i); 13 | // These indices tell us the order of R and c. 14 | Eigen::VectorXi idxs = spline.segmentVvCoefficientVectorIndices(i); 15 | 16 | std::vector< DesignVariable * > dvs; 17 | for(unsigned i = 0; i < idxs.size(); ++i) { 18 | dvs.push_back(splineDv.designVariable(idxs[i])); 19 | } 20 | MarginalizationPriorErrorTerm::Ptr err( 21 | new MarginalizationPriorErrorTerm( dvs, R*c, R )); 22 | problem.addErrorTerm(err); 23 | 24 | } 25 | } 26 | 27 | 28 | } // namespace backend 29 | } // namespace aslam 30 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/backend/implementation/CovarianceReprojectionError.hpp: -------------------------------------------------------------------------------- 1 | 2 | namespace aslam { 3 | namespace backend { 4 | 5 | template 6 | CovarianceReprojectionError::CovarianceReprojectionError() 7 | { 8 | 9 | } 10 | 11 | 12 | template 13 | CovarianceReprojectionError::CovarianceReprojectionError(const frame_t * frame, int keypointIndex, 14 | HomogeneousExpression point, CameraDesignVariable camera, spline_t* spline, Scalar* lineDelayDv) : 15 | _frame(frame), _keypointIndex(keypointIndex), _point(point), _camera(camera), _spline(spline), _lineDelayDv(lineDelayDv) 16 | { 17 | SM_ASSERT_TRUE(Exception, frame != NULL, "The frame must not be null"); 18 | // if a spline is given, estimate the covariance in each iteration 19 | //if(!spline) 20 | // parent_t::_invR = _frame->keypoint(_keypointIndex).invR(); 21 | DesignVariable::set_t dvs; 22 | point.getDesignVariables(dvs); // point dv's 23 | camera.getDesignVariables(dvs); // camera dv's 24 | 25 | parent_t::setDesignVariablesIterator(dvs.begin(), dvs.end()); 26 | 27 | } 28 | 29 | 30 | template 31 | CovarianceReprojectionError::~CovarianceReprojectionError() 32 | { 33 | 34 | } 35 | 36 | template 37 | Eigen::MatrixXd CovarianceReprojectionError::covarianceMatrix() { 38 | 39 | const keypoint_t & k = _frame->keypoint(_keypointIndex); 40 | const camera_geometry_t & cam = _frame->geometry(); 41 | 42 | Eigen::Vector4d p = _point.toHomogeneous(); 43 | measurement_t hat_y; 44 | Eigen::Matrix outJp; 45 | cam.homogeneousToKeypoint(p, hat_y, outJp); 46 | 47 | 48 | double lineDelay; 49 | if(_lineDelayDv) 50 | lineDelay = _lineDelayDv->toScalar(); 51 | else 52 | lineDelay = cam.shutter().lineDelay(); 53 | 54 | double observationTime = _frame->keypointTime(_keypointIndex).toSec() + k.y()(1) * lineDelay; 55 | 56 | Eigen::VectorXd splinePoint = _spline->spline().evalD(observationTime, 0); 57 | // evaluate the covariance: 58 | Eigen::MatrixXd JT; 59 | Eigen::VectorXd Phi_dot_c = _spline->spline().evalD(observationTime,1); // phi_dot * c (t_0) 60 | Eigen::MatrixXd T = _spline->spline().curveValueToTransformationAndJacobian(splinePoint, &JT); 61 | 62 | Eigen::MatrixXd TPboxminus = sm::kinematics::boxMinus(T*p); 63 | 64 | //std::cout << "TPboxminus:" << std::endl << TPboxminus << std::endl; 65 | 66 | Eigen::MatrixXd J = outJp * TPboxminus * JT * Phi_dot_c * lineDelay; //outJp * J_t; 67 | //std::cout << "J: " << std::endl << J << std::endl; 68 | 69 | Eigen::MatrixXd A = Eigen::MatrixXd::Identity(2,2); 70 | 71 | A(0,1) += J(0); 72 | A(1,1) += J(1); 73 | 74 | // make sure, the variance of v remains positive and set to 0 if it is negative 75 | 76 | // if (A(1,1) < 0) { 77 | // std::cout << "set one of them to zero..." << std::endl; 78 | // A(1,1) = 0; ======> THIS IS BAD! 79 | // } 80 | 81 | return A; 82 | 83 | } 84 | 85 | 86 | template 87 | double CovarianceReprojectionError::evaluateErrorImplementation() 88 | { 89 | const keypoint_t & k = _frame->keypoint(_keypointIndex); 90 | const camera_geometry_t & cam = _frame->geometry(); 91 | 92 | Eigen::Vector4d p = _point.toHomogeneous(); 93 | measurement_t hat_y; 94 | Eigen::Matrix outJp; 95 | cam.homogeneousToKeypoint(p, hat_y, outJp); 96 | 97 | parent_t::setError(k.y() - hat_y); 98 | 99 | if(_spline) { 100 | 101 | Eigen::MatrixXd A = covarianceMatrix(); 102 | //std::cout << A << std::endl; 103 | 104 | parent_t::setInvR ( ((A*_frame->keypoint(_keypointIndex).invR().inverse()*A.transpose()).inverse())); // invert 105 | // setInvR(A*_frame->keypoint(_keypointIndex).invR()*A.transpose()); 106 | 107 | // std::cout << "A: " << std::endl << A << std::endl ; 108 | //std::cout << "ARAT: " << std::endl << A*_frame->keypoint(_keypointIndex).invR()*A.transpose() << std::endl << std::endl; 109 | } 110 | 111 | return parent_t::error().dot(parent_t::invR() * parent_t::error()); 112 | } 113 | 114 | template 115 | double CovarianceReprojectionError::observationTime() 116 | { 117 | double lineDelay; 118 | if(_lineDelayDv) 119 | lineDelay = _lineDelayDv->toScalar(); 120 | else 121 | lineDelay = _frame->geometry().shutter().lineDelay(); 122 | return _frame->keypointTime(_keypointIndex).toSec() + _frame->keypoint(_keypointIndex).y()(1) * lineDelay; 123 | } 124 | 125 | 126 | 127 | template 128 | void CovarianceReprojectionError::evaluateJacobiansImplementation(aslam::backend::JacobianContainer & _jacobians) 129 | { 130 | //const keypoint_t & k = _frame->keypoint(_keypointIndex); 131 | const camera_geometry_t & cam = _frame->geometry(); 132 | 133 | Eigen::Vector4d p = _point.toHomogeneous(); 134 | typename camera_geometry_t::jacobian_homogeneous_t J; 135 | measurement_t hat_y; 136 | cam.homogeneousToKeypoint(p, hat_y, J); 137 | 138 | _point.evaluateJacobians(_jacobians, -J); 139 | 140 | _camera.evaluateJacobians(_jacobians, p); 141 | 142 | } 143 | 144 | 145 | } // namespace backend 146 | } // namespace aslam 147 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/backend/implementation/SimpleSplineError.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | namespace aslam { 4 | namespace backend { 5 | 6 | 7 | 8 | template 9 | SimpleSplineError::SimpleSplineError(spline_t* splineDV, expression_t* splineExpression, Eigen::Matrix y, double t): 10 | _splineDV(splineDV), _splineExpression(splineExpression), _y(y), _t(t) 11 | { 12 | 13 | // Add the design variables to the error term: 14 | DesignVariable::set_t dvV; 15 | //for ( unsigned int i = 0; i < _splineExpression->numDesignVariables(); i++) { 16 | // dvV.push_back(_splineExpression->designVariable(i)); 17 | //} 18 | _splineExpression->getDesignVariables(dvV); 19 | setDesignVariablesIterator(dvV.begin(), dvV.end()); 20 | 21 | } 22 | 23 | 24 | template 25 | SimpleSplineError::~SimpleSplineError() 26 | { 27 | 28 | 29 | 30 | } 31 | 32 | 33 | /// \brief evaluate the error term and return the weighted squared error e^T invR e 34 | template 35 | double SimpleSplineError::evaluateErrorImplementation() 36 | { 37 | Eigen::VectorXd error = (_splineExpression->toValue() - _y); 38 | parent_t::setError(error); 39 | return error.transpose()*error; 40 | 41 | } 42 | 43 | 44 | /// \brief evaluate the jacobians 45 | template 46 | void SimpleSplineError::evaluateJacobiansImplementation(aslam::backend::JacobianContainer & _jacobians) 47 | { 48 | 49 | _splineExpression->evaluateJacobians(_jacobians); 50 | 51 | 52 | 53 | } 54 | 55 | 56 | 57 | 58 | 59 | } // namespace backend 60 | } // namespace aslam 61 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/BSplineDesignVariable.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_SPLINES_BSPLINE_DESIGN_VARIABLE_HPP 2 | #define ASLAM_SPLINES_BSPLINE_DESIGN_VARIABLE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "BSplineExpressions.hpp" 14 | namespace aslam { 15 | namespace splines { 16 | 17 | 18 | template 19 | class BSplineDesignVariable 20 | { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | 24 | enum { 25 | Dimension = DIM 26 | }; 27 | 28 | typedef aslam::backend::DesignVariableMappedVector dv_t; 29 | 30 | /// \brief this guy takes a copy. 31 | BSplineDesignVariable(const bsplines::BSpline & bspline); 32 | 33 | virtual ~BSplineDesignVariable(); 34 | 35 | /// \brief get the spline. 36 | const bsplines::BSpline & spline(); 37 | 38 | /// \brief get an expression 39 | aslam::backend::VectorExpression toExpression(double time, int derivativeOrder); 40 | 41 | size_t numDesignVariables(); 42 | aslam::backend::DesignVariableMappedVector * designVariable(size_t i); 43 | 44 | std::vector getDesignVariables(double time) const; 45 | 46 | // Fabio: 47 | // add one Segment at the end of the PoseSpline 48 | void addSegment(double t, const Eigen::VectorXd & p); 49 | void addSegment2(double t, const Eigen::VectorXd & p, double lambda); 50 | void removeSegment(); 51 | 52 | protected: 53 | /// \brief the internal spline. 54 | bsplines::BSpline _bspline; 55 | 56 | /// \brief the vector of design variables. 57 | boost::ptr_vector< dv_t > _designVariables; 58 | 59 | }; 60 | 61 | } // namespace splines 62 | } // namespace aslam 63 | 64 | #include "implementation/BSplineDesignVariable.hpp" 65 | 66 | #endif /* ASLAM_SPLINES_BSPLINE_DESIGN_VARIABLE_HPP */ 67 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/BSplinePoseDesignVariable.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_SPLINES_BSPLINE_POSE_DESIGN_VARIABLE_HPP 2 | #define ASLAM_SPLINES_BSPLINE_POSE_DESIGN_VARIABLE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace aslam { 15 | namespace splines { 16 | 17 | 18 | class BSplinePoseDesignVariable 19 | { 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | typedef aslam::backend::DesignVariableMappedVector<6> dv_t; 24 | 25 | /// \brief this guy takes a copy. 26 | BSplinePoseDesignVariable(const bsplines::BSplinePose & bsplinePose); 27 | 28 | virtual ~BSplinePoseDesignVariable(); 29 | 30 | /// \brief get the spline. 31 | const bsplines::BSplinePose & spline(); 32 | 33 | // \todo Return a Transformation expression, a Rotation expression, A Euclidean point expression, and lots of VectorExpressions. 34 | aslam::backend::TransformationExpression transformation(double tk); 35 | aslam::backend::RotationExpression orientation(double tk); 36 | aslam::backend::EuclideanExpression position(double tk); 37 | 38 | // Get a transformation expression where the time may change. 39 | aslam::backend::TransformationExpression transformationAtTime(const aslam::backend::ScalarExpression & time, double leftBuffer, double rightBuffer); 40 | aslam::backend::TransformationExpression transformationAtTime(const aslam::backend::ScalarExpression & time); 41 | 42 | // Fabio: 43 | aslam::backend::EuclideanExpression linearVelocity(double tk); 44 | aslam::backend::EuclideanExpression linearAcceleration(double tk); 45 | aslam::backend::EuclideanExpression angularVelocityBodyFrame(double tk); 46 | // Fabio: 47 | aslam::backend::EuclideanExpression angularAccelerationBodyFrame(double tk); 48 | 49 | aslam::backend::EuclideanExpression linearAccelerationBodyFrame(double tk); 50 | 51 | size_t numDesignVariables(); 52 | aslam::backend::DesignVariableMappedVector<6> * designVariable(size_t i); 53 | 54 | 55 | // Luc: 56 | Eigen::VectorXi getActiveDesignVariableIndices(double tk); 57 | // Fabio: 58 | std::vector getDesignVariables(double tk); 59 | 60 | 61 | // Fabio: 62 | // add one Segment at the end of the PoseSpline 63 | void addSegment(double t, Eigen::Matrix4d T); 64 | void addSegment2(double t, Eigen::Matrix4d T, double lambda); 65 | void removeSegment(); 66 | 67 | private: 68 | /// \brief the internal spline. 69 | bsplines::BSplinePose _bsplinePose; 70 | 71 | /// \brief the vector of design variables. 72 | boost::ptr_vector< dv_t > _designVariables; 73 | 74 | }; 75 | 76 | } // namespace splines 77 | } // namespace aslam 78 | 79 | #endif /* ASLAM_SPLINES_BSPLINE_POSE_DESIGN_VARIABLE_HPP */ 80 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/BSplineRSExpressions.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_SPLINES_B_SPLINE_RS_EXPRESSIONS_HPP 2 | #define ASLAM_SPLINES_B_SPLINE_RS_EXPRESSIONS_HPP 3 | 4 | 5 | #include 6 | #include 7 | //#include 8 | //#include 9 | #include 10 | #include 11 | //#include 12 | //#include 13 | #include 14 | #include 15 | 16 | 17 | namespace aslam { 18 | namespace splines { 19 | class BSplinePoseDesignVariable; 20 | class BSplineRSPoseDesignVariable; 21 | 22 | 23 | // aslam::backend::TransformationExpression transformation(double tk); 24 | class RSLineDelayTransformationExpressionNode : public aslam::backend::TransformationExpressionNode 25 | { 26 | public: 27 | 28 | RSLineDelayTransformationExpressionNode(BSplinePoseDesignVariable * bspline, aslam::backend::DesignVariableVector<1> * lineDelay, double integrationStartTime, double line); 29 | virtual ~RSLineDelayTransformationExpressionNode(); 30 | 31 | protected: 32 | virtual Eigen::Matrix4d toTransformationMatrixImplementation() override; 33 | virtual void evaluateJacobiansImplementation(aslam::backend::JacobianContainer & outJacobians) const override; 34 | virtual void getDesignVariablesImplementation(aslam::backend::DesignVariable::set_t & designVariables) const override; 35 | 36 | 37 | BSplinePoseDesignVariable * _spline; 38 | aslam::backend::DesignVariableVector<1> * _lineDelay; 39 | double _integrationStartTime; 40 | double _line; 41 | 42 | }; 43 | 44 | } // namespace splines 45 | } // namespace aslam 46 | 47 | //#include "implementation/BSplineExpressions.hpp" 48 | 49 | #endif /* ASLAM_SPLINES_B_SPLINE_RS_EXPRESSIONS_HPP */ 50 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/BSplineRSPoseDesignVariable.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_SPLINES_BSPLINE_RS_POSE_DESIGN_VARIABLE_HPP 2 | #define ASLAM_SPLINES_BSPLINE_RS_POSE_DESIGN_VARIABLE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | //#include 10 | //#include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | 17 | 18 | 19 | namespace aslam { 20 | namespace splines { 21 | 22 | 23 | class BSplineRSPoseDesignVariable 24 | { 25 | public: 26 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 27 | typedef Eigen::Matrix vector_t; 28 | 29 | 30 | /// \brief this guy takes a copy 31 | BSplineRSPoseDesignVariable(const bsplines::BSplinePose & bsplinePose, const double lineDelay); 32 | 33 | virtual ~BSplineRSPoseDesignVariable(); 34 | 35 | /// \brief get the spline. 36 | const bsplines::BSplinePose & spline(); 37 | double lineDelay(); 38 | 39 | // \todo Return a Transformation expression, a Rotation expression, A Euclidean point expression, and lots of VectorExpressions. 40 | aslam::backend::TransformationExpression transformation(double t0, double line); 41 | // aslam::backend::VectorExpression<1> lineDelay(); 42 | 43 | 44 | size_t numDesignVariables(); 45 | /// \brief Design variables involving splines must have the spline design variables in the FIRST positions! (important for BSplineMotion Error) 46 | aslam::backend::DesignVariable * designVariable(size_t i); 47 | 48 | // 49 | Eigen::VectorXi getActiveDesignVariableIndices(double tk); 50 | 51 | private: 52 | /// \brief the internal spline. 53 | bsplines::BSplinePose _bsplinePose; 54 | vector_t _lineDelay; 55 | 56 | // design variables 57 | BSplinePoseDesignVariable* _bsplinePoseDV; 58 | aslam::backend::DesignVariableVector<1> * _lineDelayDV; 59 | 60 | }; 61 | 62 | } // namespace splines 63 | } // namespace aslam 64 | 65 | #endif /* ASLAM_SPLINES_BSPLINE_RS_POSE_DESIGN_VARIABLE_HPP */ 66 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/EuclideanBSplineDesignVariable.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_SPLINES_EUCLIDEAN_SPLINE_HPP 2 | #define ASLAM_SPLINES_EUCLIDEAN_SPLINE_HPP 3 | 4 | #include "BSplineDesignVariable.hpp" 5 | #include 6 | 7 | namespace aslam { 8 | namespace splines { 9 | 10 | class EuclideanBSplineDesignVariable : public BSplineDesignVariable<3> 11 | { 12 | public: 13 | EuclideanBSplineDesignVariable(const bsplines::BSpline & bspline); 14 | virtual ~EuclideanBSplineDesignVariable(); 15 | 16 | aslam::backend::EuclideanExpression toEuclideanExpression(double time, int order); 17 | 18 | Eigen::Vector3d toEuclidean(double time, int order); 19 | }; 20 | 21 | } // namespace splines 22 | } // namespace aslam 23 | 24 | #endif /* ASLAM_SPLINES_EUCLIDEAN_SPLINE_HPP */ 25 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/EuclideanSpline.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ASLAM_SPLINES_EUCLIDEAN_SPLINE_HPP 2 | #define ASLAM_SPLINES_EUCLIDEAN_SPLINE_HPP 3 | 4 | #include "BSplineDesignVariable.hpp" 5 | 6 | namespace aslam { 7 | namespace splines { 8 | 9 | class EuclideanSplineDesignVariable : public 10 | { 11 | public: 12 | EuclideanSplineDesignVariable(); 13 | virtual ~EuclideanSplineDesignVariable(); 14 | }; 15 | 16 | } // namespace splines 17 | } // namespace aslam 18 | 19 | #endif /* ASLAM_SPLINES_EUCLIDEAN_SPLINE_HPP */ 20 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/OPTEuclideanBSpline.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OPTBSpline.hpp 3 | * 4 | * Created on: 05.08.2012 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef OPTEUCLIDEANBSPLINE_HPP_ 9 | #define OPTEUCLIDEANBSPLINE_HPP_ 10 | 11 | #include "OPTBSpline.hpp" 12 | #include "bsplines/EuclideanBSpline.hpp" 13 | 14 | namespace bsplines { 15 | 16 | template 17 | class DiffManifoldBSpline, TModifiedDerivedConf>, aslam::splines::DesignVariableSegmentBSplineConf > 18 | : public DiffManifoldBSpline::ParentConf, TModifiedDerivedConf>, aslam::splines::DesignVariableSegmentBSplineConf >{ 19 | protected: 20 | typedef DiffManifoldBSpline::ParentConf, TModifiedDerivedConf>, aslam::splines::DesignVariableSegmentBSplineConf > parent_t; 21 | typedef aslam::splines::DesignVariableSegmentBSplineConf CONF; 22 | public: 23 | DiffManifoldBSpline(const CONF & config = CONF()) : parent_t(config){} 24 | DiffManifoldBSpline(int splineOrder, int dimension = parent_t::Dimension) : parent_t(typename CONF::ParentConf(typename CONF::ManifoldConf(dimension), splineOrder)){} 25 | 26 | }; 27 | 28 | } // namespace bsplines 29 | 30 | #endif /* OPTEUCLIDEANBSPLINE_HPP_ */ 31 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/OPTUnitQuaternionBSpline.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OPTBSpline.hpp 3 | * 4 | * Created on: 05.08.2012 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef OPTUNITQUATERNIONBSPLINE_HPP_ 9 | #define OPTUNITQUATERNIONBSPLINE_HPP_ 10 | 11 | #include "OPTBSpline.hpp" 12 | #include "bsplines/UnitQuaternionBSpline.hpp" 13 | 14 | namespace bsplines { 15 | 16 | template 17 | class DiffManifoldBSpline, TModifiedDerivedConf>, aslam::splines::DesignVariableSegmentBSplineConf > 18 | : public DiffManifoldBSpline::ParentConf, TModifiedDerivedConf>, aslam::splines::DesignVariableSegmentBSplineConf >{ 19 | protected: 20 | typedef DiffManifoldBSpline::ParentConf, TModifiedDerivedConf>, aslam::splines::DesignVariableSegmentBSplineConf > parent_t; 21 | typedef aslam::splines::DesignVariableSegmentBSplineConf CONF; 22 | private: 23 | typedef typename ::bsplines::UnitQuaternionBSpline::TYPE TBSpline; 24 | public: 25 | typedef typename parent_t::spline_t spline_t; 26 | typedef typename parent_t::TimePolicy TimePolicy; 27 | typedef typename parent_t::time_t time_t; 28 | typedef typename parent_t::point_t point_t; 29 | typedef typename parent_t::SegmentIterator SegmentIterator; 30 | typedef typename parent_t::SegmentConstIterator SegmentConstIterator; 31 | typedef typename parent_t::angular_jacobian_t angular_jacobian_t; 32 | 33 | enum { 34 | PointSize = TBSpline::PointSize, 35 | Dimension = TBSpline::Dimension 36 | }; 37 | 38 | typedef aslam::backend::VectorExpression<3> angular_derivative_expression_t; 39 | 40 | DiffManifoldBSpline(const CONF & config = CONF()) : parent_t(config){} 41 | DiffManifoldBSpline(int splineOrder) : parent_t(typename CONF::ParentConf(splineOrder)){} 42 | 43 | template 44 | class ExpressionFactory : public parent_t::template ExpressionFactory { 45 | public: 46 | typedef typename parent_t::template ExpressionFactory ParentExpressionFactory; 47 | typedef typename ParentExpressionFactory::DataSharedPtr DataSharedPtr; 48 | typedef typename FactoryData_::eval_t eval_t; 49 | 50 | /// \brief get an expression 51 | angular_derivative_expression_t getAngularVelocityExpression() const; 52 | angular_derivative_expression_t getAngularAccelerationExpression() const; 53 | protected: 54 | inline ExpressionFactory(const FactoryData_ & factoryBase) : ParentExpressionFactory(factoryBase) {} 55 | friend class DiffManifoldBSpline; 56 | }; 57 | 58 | template inline ExpressionFactory > getExpressionFactoryAt(const time_t & t) const { 59 | return ExpressionFactory >(typename parent_t::template ConstTimeFactoryData(*this, t)); 60 | } 61 | template inline ExpressionFactory > getExpressionFactoryAt(const typename parent_t::TimeExpression & t, time_t lowerBound, time_t upperBound) const { 62 | return ExpressionFactory >(typename parent_t::template TimeExpressionFactoryData(*this, t, lowerBound, upperBound)); 63 | } 64 | }; 65 | 66 | } // namespace bsplines 67 | 68 | #include "implementation/OPTUnitQuaternionBSplineImpl.hpp" 69 | 70 | #endif /* OPTUNITQUATERNIONBSPLINE_HPP_ */ 71 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/implementation/BSplineDesignVariable.hpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | namespace aslam { 4 | namespace splines { 5 | 6 | /// \brief this guy takes a copy. 7 | template 8 | BSplineDesignVariable::BSplineDesignVariable(const bsplines::BSpline & bspline) : 9 | _bspline(bspline) 10 | { 11 | // here is where the magic happens. 12 | 13 | // Create all of the design variables as maps into the vector of spline coefficients. 14 | for(int i = 0; i < _bspline.numVvCoefficients(); ++i) 15 | { 16 | _designVariables.push_back( new aslam::backend::DesignVariableMappedVector( _bspline.fixedSizeVvCoefficientVector(i) ) ); 17 | } 18 | } 19 | 20 | template 21 | BSplineDesignVariable::~BSplineDesignVariable() 22 | { 23 | 24 | } 25 | 26 | /// \brief get the spline. 27 | template 28 | const bsplines::BSpline & BSplineDesignVariable::spline() 29 | { 30 | return _bspline; 31 | } 32 | 33 | template 34 | size_t BSplineDesignVariable::numDesignVariables() 35 | { 36 | return _designVariables.size(); 37 | } 38 | 39 | template 40 | aslam::backend::DesignVariableMappedVector * BSplineDesignVariable::designVariable(size_t i) 41 | { 42 | SM_ASSERT_LT(aslam::Exception, i, _designVariables.size(), "Index out of bounds"); 43 | return &_designVariables[i]; 44 | } 45 | 46 | template 47 | aslam::backend::VectorExpression BSplineDesignVariable::toExpression(double tk, int derivativeOrder) 48 | { 49 | Eigen::VectorXi dvidxs = _bspline.localVvCoefficientVectorIndices(tk); 50 | std::vector dvs; 51 | for(int i = 0; i < dvidxs.size(); ++i) 52 | { 53 | dvs.push_back(&_designVariables[dvidxs[i]]); 54 | } 55 | boost::shared_ptr > root( new aslam::splines::BSplineVectorExpressionNode(&_bspline, derivativeOrder, dvs, tk) ); 56 | 57 | return aslam::backend::VectorExpression(root); 58 | 59 | } 60 | 61 | template 62 | std::vector BSplineDesignVariable::getDesignVariables(double tk) const 63 | { 64 | Eigen::VectorXi dvidxs = _bspline.localVvCoefficientVectorIndices(tk); 65 | std::vector dvs; 66 | for(int i = 0; i < dvidxs.size(); ++i) 67 | { 68 | dvs.push_back(const_cast*>(&_designVariables[dvidxs[i]])); 69 | } 70 | 71 | return dvs; 72 | } 73 | 74 | 75 | template 76 | void BSplineDesignVariable::addSegment(double t, const Eigen::VectorXd & p) 77 | { 78 | _bspline.addCurveSegment(t, p); 79 | _designVariables.push_back( new aslam::backend::DesignVariableMappedVector( _bspline.fixedSizeVvCoefficientVector(_bspline.numVvCoefficients()-1) ) ); 80 | for(int i = 0; i < _bspline.numVvCoefficients()-1; i++) 81 | { 82 | _designVariables[i].updateMap(_bspline.fixedSizeVvCoefficientVector(i).data()); 83 | } 84 | } 85 | 86 | 87 | template 88 | void BSplineDesignVariable::addSegment2(double t, const Eigen::VectorXd & p, double lambda) 89 | { 90 | _bspline.addCurveSegment2(t,p,lambda); 91 | _designVariables.push_back( new aslam::backend::DesignVariableMappedVector( _bspline.fixedSizeVvCoefficientVector(_bspline.numVvCoefficients()-1) ) ); 92 | for(int i = 0; i < _bspline.numVvCoefficients()-1; i++) 93 | { 94 | _designVariables[i].updateMap(_bspline.fixedSizeVvCoefficientVector(i).data()); 95 | } } 96 | 97 | 98 | template 99 | void BSplineDesignVariable::removeSegment() 100 | { 101 | _bspline.removeCurveSegment(); 102 | _designVariables.erase(_designVariables.begin()); 103 | for(int i = 0; i < _bspline.numVvCoefficients(); i++) 104 | { 105 | _designVariables[i].updateMap(_bspline.fixedSizeVvCoefficientVector(i).data()); 106 | } 107 | } 108 | 109 | } // namespace splines 110 | } // namespace aslam 111 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/implementation/BSplineExpressions.hpp: -------------------------------------------------------------------------------- 1 | namespace aslam { 2 | namespace splines { 3 | 4 | 5 | template 6 | BSplineVectorExpressionNode::BSplineVectorExpressionNode(bsplines::BSpline * spline, int derivativeOrder, const std::vector & designVariables, double time) : 7 | _spline(spline), _designVariables(designVariables), _time(time), _derivativeOrder(derivativeOrder) 8 | { 9 | SM_ASSERT_EQ(aslam::Exception, spline->coefficients().rows(), D, "The spline dimension should match the expression dimension"); 10 | } 11 | 12 | template 13 | BSplineVectorExpressionNode::~BSplineVectorExpressionNode() 14 | { 15 | 16 | } 17 | 18 | template 19 | typename BSplineVectorExpressionNode::vector_t BSplineVectorExpressionNode::evaluateImplementation() const 20 | { 21 | return _spline->evalD(_time, _derivativeOrder); 22 | } 23 | 24 | 25 | template 26 | void BSplineVectorExpressionNode::evaluateJacobiansImplementation(aslam::backend::JacobianContainer & outJacobians) const 27 | { 28 | Eigen::MatrixXd J; 29 | _spline->evalDAndJacobian(_time, _derivativeOrder, &J, NULL); 30 | 31 | for(size_t i = 0; i < _designVariables.size(); ++i) 32 | { 33 | outJacobians.add(_designVariables[i], J.block(0,i*D) ); 34 | } 35 | } 36 | 37 | template 38 | void BSplineVectorExpressionNode::getDesignVariablesImplementation(aslam::backend::DesignVariable::set_t & designVariables) const 39 | { 40 | for(size_t i = 0; i < _designVariables.size(); ++i) 41 | { 42 | designVariables.insert(_designVariables[i]); 43 | } 44 | 45 | } 46 | 47 | 48 | } // namespace splines 49 | } // namespace aslam 50 | -------------------------------------------------------------------------------- /aslam_splines/include/aslam/splines/implementation/OPTUnitQuaternionBSplineImpl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef OPTUNITQUATERNIONBSPLINEIMPL_HPP_ 2 | #define OPTUNITQUATERNIONBSPLINEIMPL_HPP_ 3 | 4 | #include "aslam/backend/JacobianContainer.hpp" 5 | 6 | namespace bsplines { 7 | 8 | #define _TEMPLATE template 9 | #define _CLASS DiffManifoldBSpline, TModifiedDerivedConf>, aslam::splines::DesignVariableSegmentBSplineConf > 10 | 11 | _TEMPLATE 12 | template 13 | typename _CLASS::angular_derivative_expression_t _CLASS::ExpressionFactory::getAngularVelocityExpression() const { 14 | typedef aslam::backend::VectorExpressionNode<3> node_t; 15 | 16 | class ExpressionNode : public node_t { 17 | public: 18 | ExpressionNode(const DataSharedPtr & dataPtr) : _dataPtr(dataPtr){} 19 | private: 20 | const DataSharedPtr _dataPtr; 21 | inline void evaluateJacobiansImplementation(aslam::backend::JacobianContainer & outJacobians, const Eigen::MatrixXd * applyChainRule) const { 22 | const int dimension=_dataPtr->getSpline().getDimension(), pointSize = dimension, splineOrder = _dataPtr->getSpline().getSplineOrder(); 23 | typename _CLASS::angular_jacobian_t J(pointSize, dimension * splineOrder); 24 | auto & eval = _dataPtr->getEvaluator(); 25 | eval.evalAngularVelocityJacobian(J); 26 | 27 | int col = 0; 28 | for(SegmentConstIterator i = eval.begin(), end = eval.end(); i != end; ++i) 29 | { 30 | internal::AddJac::addJac(J, col, &i->getDesignVariable(), outJacobians, applyChainRule, pointSize, dimension); 31 | col+=dimension; 32 | } 33 | if(_dataPtr->hasTimeExpression()){ 34 | auto evalJac = eval.evalAngularAcceleration(); 35 | _dataPtr->getTimeExpression().evaluateJacobians(outJacobians, applyChainRule ? Eigen::MatrixXd(*applyChainRule * evalJac) : Eigen::MatrixXd(evalJac)); 36 | } 37 | } 38 | 39 | protected: 40 | virtual node_t::vector_t evaluateImplementation() const { 41 | return _dataPtr->getEvaluator().evalAngularVelocity(); 42 | } 43 | 44 | virtual void evaluateJacobiansImplementation(aslam::backend::JacobianContainer & outJacobians) const { 45 | evaluateJacobiansImplementation(outJacobians, NULL); 46 | } 47 | 48 | virtual void evaluateJacobiansImplementation(aslam::backend::JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const { 49 | evaluateJacobiansImplementation(outJacobians, &applyChainRule); 50 | } 51 | 52 | virtual void getDesignVariablesImplementation(aslam::backend::DesignVariable::set_t & designVariables) const { 53 | _dataPtr->getDesignVariables(designVariables); 54 | } 55 | }; 56 | return angular_derivative_expression_t(boost::shared_ptr(static_cast (new ExpressionNode(this->getDataPtr())))); 57 | } 58 | 59 | _TEMPLATE 60 | template 61 | typename _CLASS::angular_derivative_expression_t _CLASS::ExpressionFactory::getAngularAccelerationExpression() const { 62 | typedef aslam::backend::VectorExpressionNode<3> node_t; 63 | 64 | class ExpressionNode : public node_t { 65 | private: 66 | _CLASS::ExpressionFactory::DataSharedPtr _dataPtr; 67 | public: 68 | ExpressionNode(const DataSharedPtr & dataPtr) : _dataPtr(dataPtr){} 69 | virtual ~ExpressionNode(){}; 70 | private: 71 | inline void evaluateJacobiansImplementation(aslam::backend::JacobianContainer & outJacobians, const Eigen::MatrixXd * applyChainRule) const { 72 | const int dimension=_dataPtr->getSpline().getDimension(), pointSize = dimension, splineOrder = _dataPtr->getSpline().getSplineOrder(); 73 | typename _CLASS::angular_jacobian_t J(pointSize, dimension * splineOrder); 74 | auto & eval = _dataPtr->getEvaluator(); 75 | 76 | eval.evalAngularAccelerationJacobian(J); 77 | int col = 0; 78 | for(SegmentConstIterator i = eval.begin(), end = eval.end(); i != end; ++i) 79 | { 80 | internal::AddJac::addJac(J, col, &i->getDesignVariable(), outJacobians, applyChainRule, pointSize, dimension); 81 | col+=dimension; 82 | } 83 | if(_dataPtr->hasTimeExpression()){ 84 | auto evalJac = eval.template evalAngularDerivative<3>(); 85 | _dataPtr->getTimeExpression().evaluateJacobians(outJacobians, applyChainRule ? Eigen::MatrixXd(*applyChainRule * evalJac) : Eigen::MatrixXd(evalJac)); 86 | } 87 | } 88 | 89 | protected: 90 | virtual node_t::vector_t evaluateImplementation() const { 91 | return _dataPtr->getEvaluator().evalAngularAcceleration(); 92 | } 93 | 94 | virtual void evaluateJacobiansImplementation(aslam::backend::JacobianContainer & outJacobians) const { 95 | evaluateJacobiansImplementation(outJacobians, NULL); 96 | } 97 | 98 | virtual void evaluateJacobiansImplementation(aslam::backend::JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const { 99 | evaluateJacobiansImplementation(outJacobians, &applyChainRule); 100 | } 101 | 102 | virtual void getDesignVariablesImplementation(aslam::backend::DesignVariable::set_t & designVariables) const { 103 | _dataPtr->getDesignVariables(designVariables); 104 | } 105 | }; 106 | return angular_derivative_expression_t(boost::shared_ptr(static_cast (new ExpressionNode(this->getDataPtr())))); 107 | } 108 | 109 | #undef _CLASS 110 | #undef _TEMPLATE 111 | 112 | } // namespace bsplines 113 | 114 | #endif /* OPTUNITQUATERNIONBSPLINEIMPL_HPP_ */ 115 | -------------------------------------------------------------------------------- /aslam_splines/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_splines 4 | aslam_splines 5 | 0.0.1 6 | Paul Furgale 7 | Paul Furgale 8 | BSD-3-Clause 9 | catkin 10 | catkin_simple 11 | 12 | aslam_backend 13 | aslam_backend_expressions 14 | bsplines 15 | sparse_block_matrix 16 | eigen_catkin 17 | sm_common 18 | sm_kinematics 19 | sm_timing 20 | 21 | aslam_backend 22 | aslam_backend_expressions 23 | bsplines 24 | sparse_block_matrix 25 | eigen_catkin 26 | sm_common 27 | sm_kinematics 28 | sm_timing 29 | 30 | 31 | -------------------------------------------------------------------------------- /aslam_splines/src/BSplineRSExpressions.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #define LINEDELAY_UNIT 1 //e-11 6 | 7 | namespace aslam { 8 | namespace splines { 9 | 10 | ///////////////////////// 11 | 12 | // linedelay in UNIT 13 | RSLineDelayTransformationExpressionNode::RSLineDelayTransformationExpressionNode(BSplinePoseDesignVariable * bspline, aslam::backend::DesignVariableVector<1>* lineDelay, double integrationStartTime, double line) 14 | { 15 | _spline = bspline; 16 | _lineDelay = lineDelay; 17 | _line = line; 18 | _integrationStartTime = integrationStartTime; 19 | } 20 | 21 | RSLineDelayTransformationExpressionNode::~RSLineDelayTransformationExpressionNode() 22 | { 23 | 24 | } 25 | 26 | 27 | Eigen::Matrix4d RSLineDelayTransformationExpressionNode::toTransformationMatrixImplementation() 28 | { 29 | 30 | double observationTime = _lineDelay->value()[0]* LINEDELAY_UNIT * _line + _integrationStartTime; 31 | return _spline->spline().transformation(observationTime); 32 | 33 | } 34 | 35 | void RSLineDelayTransformationExpressionNode::evaluateJacobiansImplementation(aslam::backend::JacobianContainer & outJacobians) const 36 | { 37 | Eigen::MatrixXd JT; 38 | Eigen::MatrixXd J; 39 | 40 | 41 | double observationTime = _lineDelay->value()[0] * LINEDELAY_UNIT * _line + _integrationStartTime; 42 | 43 | Eigen::VectorXi dvidxs = _spline->spline().localVvCoefficientVectorIndices(observationTime); 44 | // _spline->spline().transformationAndJacobian(observationTime, &J); 45 | Eigen::MatrixXd JS; 46 | Eigen::VectorXd p; 47 | 48 | p = _spline->spline().evalDAndJacobian(observationTime,0,&JS, NULL); 49 | 50 | _spline->spline().curveValueToTransformationAndJacobian( p, &JT ); 51 | J = JT * JS; 52 | 53 | for(int i = 0; i < dvidxs.size(); ++i) 54 | { 55 | outJacobians.add(_spline->designVariable(dvidxs[i]), J.block<6,6>(0,i*6) ); 56 | } 57 | // evaluate time derivative of the curves 58 | Eigen::VectorXd Phi_dot_c = _spline->spline().evalD(observationTime,1); // phi_dot * c (t_0) 59 | 60 | // Add the jacobians wrt line delay: \mbf S_T * \mbsdot \Phi_dot(t) * c * p_{i,v} 61 | outJacobians.add(_lineDelay, JT * Phi_dot_c * _line * LINEDELAY_UNIT); 62 | } 63 | 64 | void RSLineDelayTransformationExpressionNode::getDesignVariablesImplementation(aslam::backend::DesignVariable::set_t & designVariables) const 65 | { 66 | double observationTime = _lineDelay->value()[0] * LINEDELAY_UNIT * _line + _integrationStartTime; 67 | Eigen::VectorXi dvidxs = _spline->spline().localVvCoefficientVectorIndices(observationTime); 68 | for(int i = 0; i < dvidxs.size(); ++i) 69 | { 70 | designVariables.insert( _spline->designVariable(dvidxs[i]) ); 71 | } 72 | designVariables.insert(_lineDelay); 73 | } 74 | 75 | 76 | 77 | } // namespace splines 78 | } // namespace aslam 79 | -------------------------------------------------------------------------------- /aslam_splines/src/BSplineRSPoseDesignVariable.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | 9 | 10 | namespace aslam { 11 | namespace splines { 12 | 13 | /// \brief this guy takes a copy. 14 | BSplineRSPoseDesignVariable::BSplineRSPoseDesignVariable(const bsplines::BSplinePose & bsplinePose, const double lineDelay) : 15 | _bsplinePose(bsplinePose) 16 | { 17 | // here is where the magic happens. 18 | _bsplinePoseDV = new BSplinePoseDesignVariable(bsplinePose); 19 | _lineDelay(0,0) = lineDelay; 20 | // _lineDelay = vector_t::Zero(); 21 | _lineDelayDV = new aslam::backend::DesignVariableVector<1>(_lineDelay); 22 | 23 | 24 | } 25 | 26 | BSplineRSPoseDesignVariable::~BSplineRSPoseDesignVariable() 27 | { 28 | 29 | } 30 | 31 | /// \brief get the spline. 32 | const bsplines::BSplinePose & BSplineRSPoseDesignVariable::spline() 33 | { 34 | return _bsplinePoseDV->spline(); 35 | } 36 | 37 | double BSplineRSPoseDesignVariable::lineDelay() { 38 | 39 | return (_lineDelayDV->value())(0,0); 40 | 41 | } 42 | 43 | size_t BSplineRSPoseDesignVariable::numDesignVariables() 44 | { 45 | return _bsplinePoseDV->numDesignVariables() + 1; 46 | } 47 | 48 | aslam::backend::DesignVariable * BSplineRSPoseDesignVariable::designVariable(size_t i) 49 | { 50 | SM_ASSERT_LT(aslam::Exception, i, numDesignVariables(), "Index out of bounds"); 51 | 52 | if ( i == numDesignVariables()-1 ) 53 | return _lineDelayDV; 54 | else 55 | return _bsplinePoseDV->designVariable(i); 56 | } 57 | 58 | Eigen::VectorXi BSplineRSPoseDesignVariable::getActiveDesignVariableIndices(double tk) 59 | { 60 | return _bsplinePose.localVvCoefficientVectorIndices(tk); 61 | } 62 | 63 | aslam::backend::TransformationExpression BSplineRSPoseDesignVariable::transformation(double t0, double line) 64 | { 65 | 66 | 67 | boost::shared_ptr root( new RSLineDelayTransformationExpressionNode( _bsplinePoseDV, _lineDelayDV, t0, line) ); 68 | 69 | return aslam::backend::TransformationExpression(root); 70 | 71 | } 72 | 73 | /* aslam::backend::VectorExpression<1> BSplineRSPoseDesignVariable::lineDelay() { 74 | 75 | boost::shared_ptr> root( new aslam::backend::VectorExpressionNode<1>( _lineDelayDV ) ); 76 | 77 | return aslam::backend::VectorExpression(root); 78 | 79 | }*/ 80 | 81 | 82 | } // namespace splines 83 | } // namespace aslam 84 | -------------------------------------------------------------------------------- /aslam_splines/src/EuclideanBSplineDesignVariable.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace aslam { 4 | namespace splines { 5 | 6 | 7 | 8 | 9 | EuclideanBSplineDesignVariable::EuclideanBSplineDesignVariable(const bsplines::BSpline & bspline) : 10 | BSplineDesignVariable<3>(bspline) 11 | { 12 | 13 | } 14 | 15 | EuclideanBSplineDesignVariable::~EuclideanBSplineDesignVariable() 16 | { 17 | 18 | } 19 | 20 | aslam::backend::EuclideanExpression EuclideanBSplineDesignVariable::toEuclideanExpression(double time, int order) 21 | { 22 | Eigen::VectorXi dvidxs = _bspline.localVvCoefficientVectorIndices(time); 23 | std::vector dvs; 24 | for(int i = 0; i < dvidxs.size(); ++i) 25 | { 26 | dvs.push_back(&_designVariables[dvidxs[i]]); 27 | } 28 | boost::shared_ptr root( new aslam::splines::BSplineEuclideanExpressionNode(&_bspline, dvs, time, order) ); 29 | 30 | return aslam::backend::EuclideanExpression(root); 31 | } 32 | 33 | 34 | Eigen::Vector3d EuclideanBSplineDesignVariable::toEuclidean(double time, int order) 35 | { 36 | return _bspline.evalD(time,order); 37 | } 38 | 39 | } // namespace splines 40 | } // namespace aslam 41 | -------------------------------------------------------------------------------- /aslam_splines/test/TestErrors.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | using namespace bsplines; 9 | using namespace aslam::splines; 10 | 11 | BSplineDesignVariable<1> generateRandomBSpline() 12 | { 13 | BSpline bspline(4); 14 | const int N = 10; 15 | Eigen::VectorXd times(N); 16 | for(int i = 0; i < N; ++i) 17 | times(i) = i; 18 | 19 | Eigen::Matrix K(1,N); 20 | K.setRandom(); 21 | 22 | 23 | bspline.initSpline3(times, K, 6, 1e-4); 24 | 25 | BSplineDesignVariable<1> bdv(bspline); 26 | for(size_t i = 0; i < bdv.numDesignVariables(); ++i) 27 | { 28 | bdv.designVariable(i)->setActive(true); 29 | bdv.designVariable(i)->setBlockIndex(i); 30 | } 31 | 32 | return bdv; 33 | 34 | } 35 | 36 | 37 | 38 | TEST(SplineErrorTestSuite, testSimpleSplineError) 39 | { 40 | try 41 | { 42 | using namespace aslam::backend; 43 | BSplineDesignVariable<1> initSpline = generateRandomBSpline(); 44 | Eigen::VectorXd values(1); 45 | values << 1.0; 46 | 47 | VectorExpression<1> splineExpression = initSpline.toExpression(5.0,0); 48 | SimpleSplineError > e(&initSpline, &splineExpression, values, 5.0); 49 | 50 | JacobianContainerSparse<> estJ(e.dimension()); 51 | e.evaluateJacobiansFiniteDifference(estJ); 52 | 53 | JacobianContainerSparse<> J(e.dimension()); 54 | e.evaluateJacobians(J); 55 | 56 | SCOPED_TRACE(""); 57 | sm::eigen::assertNear(J.asDenseMatrix(), estJ.asDenseMatrix(), 1e-6, SM_SOURCE_FILE_POS, "Checking the jacobian vs. finite differences"); 58 | 59 | } 60 | catch(const std::exception & e) 61 | { 62 | FAIL() << e.what(); 63 | } 64 | } 65 | 66 | 67 | -------------------------------------------------------------------------------- /aslam_splines/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /aslam_splines_python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(aslam_splines_python) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | # enable warnings 8 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 9 | if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 10 | # deprecated-register warnings are cause by old Eigen versions 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-register") 12 | endif() 13 | 14 | # enable C++11 support 15 | if(CMAKE_VERSION VERSION_LESS "3.1") 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 17 | else() 18 | set(CMAKE_CXX_STANDARD 11) 19 | endif() 20 | 21 | add_python_export_library(${PROJECT_NAME} python/aslam_splines 22 | src/spline_module.cpp 23 | src/BSplineMotionError.cpp 24 | src/SimpleSplineError.cpp 25 | src/OPTBSpline.cpp 26 | src/QuadraticIntegralError.cpp 27 | ) 28 | 29 | catkin_add_nosetests( 30 | test/OptBSpline.py 31 | ) 32 | catkin_add_nosetests( 33 | test/QuadraticIntegralError.py 34 | ) 35 | 36 | cs_install() 37 | cs_export() 38 | -------------------------------------------------------------------------------- /aslam_splines_python/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | aslam_splines_python 4 | 0.0.1 5 | aslam_splines_python 6 | Paul Furgale 7 | Paul Furgale 8 | BSD-3-Clause 9 | catkin 10 | catkin_simple 11 | 12 | aslam_splines 13 | bsplines_python 14 | bsplines 15 | numpy_eigen 16 | catkin_boost_python_buildtool 17 | eigen_catkin 18 | sparse_block_matrix 19 | sm_common 20 | sm_kinematics 21 | aslam_backend_expressions 22 | aslam_backend 23 | aslam_backend_python 24 | sm_timing 25 | sm_opencv 26 | sm_property_tree 27 | sm_logging 28 | 29 | -------------------------------------------------------------------------------- /aslam_splines_python/python/aslam_splines/__init__.py: -------------------------------------------------------------------------------- 1 | # Import the numpy to Eigen type conversion. 2 | import numpy_eigen 3 | import os 4 | import aslam_backend 5 | import bsplines 6 | from .libaslam_splines_python import * 7 | 8 | 9 | -------------------------------------------------------------------------------- /aslam_splines_python/setup.py: -------------------------------------------------------------------------------- 1 | 2 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 3 | 4 | from distutils.core import setup 5 | from catkin_pkg.python_setup import generate_distutils_setup 6 | 7 | # fetch values from package.xml 8 | setup_args = generate_distutils_setup( 9 | packages=['aslam_splines'], 10 | package_dir={'':'python'}) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /aslam_splines_python/src/BSplineMotionError.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | 12 | 13 | using namespace boost::python; 14 | using namespace aslam::backend; 15 | 16 | void exportBSplineMotionError() 17 | { 18 | 19 | def("addMotionErrorTerms", &addMotionErrorTerms, "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)"); 20 | def("addMotionErrorTerms", &addMotionErrorTerms, "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)"); 21 | def("addMotionErrorTerms", &addMotionErrorTerms >, "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)"); 22 | def("addMotionErrorTerms", &addMotionErrorTerms >, "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)"); 23 | def("addMotionErrorTerms", &addMotionErrorTerms >, "void addMotionErrorTerms( OptimizationProblemBase, SplineDv, W, errorTermOrder)"); 24 | 25 | 26 | class_, boost::shared_ptr >, bases > 27 | ("BSplineEuclideanMotionError", init("BSplineEuclideanMotionError(EuclideanBSplineDesignVariable, W)")) 28 | .def(init("BSplineGenericMotionError(EuclideanBSplineDesignVariable, W, errorTermOrder)")) 29 | ; 30 | using namespace aslam::splines; 31 | class_ >, boost::shared_ptr > >, bases > 32 | ("BSpline1MotionError", init *, Eigen::MatrixXd >("BSpline1MotionError(BSpline1DesignVariable, W)")) 33 | .def(init*, Eigen::MatrixXd, unsigned int >("BSpline1MotionError(BSpline1DesignVariable, W, errorTermOrder)")) 34 | ; 35 | class_ >, boost::shared_ptr > >, bases > 36 | ("BSpline2MotionError", init *, Eigen::MatrixXd >("BSpline2MotionError(BSpline2DesignVariable, W)")) 37 | .def(init*, Eigen::MatrixXd, unsigned int >("BSpline2MotionError(BSpline2DesignVariable, W, errorTermOrder)")) 38 | ; 39 | 40 | class_ >, boost::shared_ptr > >, bases > 41 | ("BSpline3MotionError", init *, Eigen::MatrixXd >("BSpline3MotionError(BSpline3DesignVariable, W)")) 42 | .def(init*, Eigen::MatrixXd, unsigned int >("BSpline3MotionError(BSpline3DesignVariable, W, errorTermOrder)")) 43 | ; 44 | 45 | 46 | 47 | 48 | class_, boost::shared_ptr >, bases > 49 | ("BSplineMotionError", init("BSplineMotionError(BSplinePoseDesignVariable, W)")) 50 | .def(init("BSplineMotionError(BSplinePoseDesignVariable, W, errorTermOrder)")) 51 | ; 52 | 53 | class_, boost::shared_ptr >, bases > 54 | ("BSplineRSMotionError", init("BSplineRSMotionError(BSplinePoseDesignVariable, W)")) 55 | .def(init("BSplineRSMotionError(BSplinePoseDesignVariable, W, errorTermOrder)")) 56 | ; 57 | 58 | 59 | } 60 | -------------------------------------------------------------------------------- /aslam_splines_python/src/OPTBSpline.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | typedef std::vector DesignVariables; 9 | 10 | using namespace bspline_exporter; 11 | using namespace bsplines; 12 | 13 | template 14 | void addOptSplineFunctions (BSplineExporter & splineExporter){ 15 | using namespace boost::python; 16 | splineExporter.getSplineClass() 17 | .def("getDesignVariables", static_cast(&Spline::getDesignVariables), "Get a list of all design variables", return_value_policy()) 18 | ; 19 | } 20 | 21 | void exportOptBSplines() { 22 | class_("DesignVariables") 23 | .def(boost::python::vector_indexing_suite() ); 24 | 25 | auto euclidBSpl = BSplineExporter::CONF>::BSpline>::exportEuclideanSpline("OptEuclideanBSpline"); 26 | addOptSplineFunctions(euclidBSpl); 27 | auto unitQuatBSpl = BSplineExporter::CONF>::BSpline>::exportUnitQuaternionSpline("OptUnitQuaternionBSpline"); 28 | addOptSplineFunctions(unitQuatBSpl); 29 | } 30 | -------------------------------------------------------------------------------- /aslam_splines_python/src/QuadraticIntegralError.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * QuadraticIntegralErrorTerm.cpp 3 | * 4 | * Created on: Dec 4, 2013 5 | * Author: hannes 6 | */ 7 | 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace boost::python; 16 | 17 | template > 18 | inline void addQuadraticIntegralExpressionErrorTerms(aslam::backend::OptimizationProblem & problem, double a, double b, int numberOfPoints, const object & expressionFactory, const Eigen::Matrix & sqrtInvR){ 19 | using namespace aslam::backend::integration; 20 | 21 | struct ExpressionFactoryAdapter { 22 | ExpressionFactoryAdapter(const object & o) : o(o){} 23 | 24 | Expression operator()(double time) const { 25 | return extract(o(time)); 26 | } 27 | private: 28 | const object & o; 29 | }; 30 | 31 | addQuadraticIntegralExpressionErrorTerms(problem, a, b, numberOfPoints, ExpressionFactoryAdapter(expressionFactory), sqrtInvR); 32 | } 33 | 34 | const char * BaseName = "addQuadraticIntegralExpressionErrorTermsToProblem"; 35 | 36 | template 37 | void defAddQuadraticIntegralExpressionErrorTerms(){ 38 | std::stringstream s; 39 | s << BaseName << dimMax; 40 | 41 | boost::python::def( 42 | s.str().c_str(), 43 | &addQuadraticIntegralExpressionErrorTerms, 44 | boost::python::args("problem, timeA, timeB, nIntegrationPoints, expressionFactory, sqrtInvR") 45 | ); 46 | defAddQuadraticIntegralExpressionErrorTerms(); 47 | } 48 | 49 | template <> 50 | void defAddQuadraticIntegralExpressionErrorTerms<0>(){ 51 | } 52 | 53 | void exportAddQuadraticIntegralExpressionErrorTerms(){ 54 | defAddQuadraticIntegralExpressionErrorTerms<3>(); 55 | 56 | boost::python::def( 57 | "addQuadraticIntegralEuclideanExpressionErrorTermsToProblem", 58 | &addQuadraticIntegralExpressionErrorTerms<3, aslam::backend::EuclideanExpression>, 59 | boost::python::args("problem, timeA, timeB, nIntegrationPoints, expressionFactory, sqrtInvR") 60 | ); 61 | } 62 | -------------------------------------------------------------------------------- /aslam_splines_python/src/SimpleSplineError.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | 8 | 9 | using namespace boost::python; 10 | using namespace aslam::splines; 11 | using namespace aslam::backend; 12 | 13 | void exportSimpleSplineError() 14 | { 15 | 16 | 17 | class_ >, boost::shared_ptr > >, bases > 18 | ("SimpleSplineError1", init*, VectorExpression<1>*, Eigen::Matrix::Dimension, 1> , double >("SimpleSplineError(BSplineDesignVariable<1>, BSPlineExpression<1>,Values, Time )")) 19 | 20 | ; 21 | // .def(init*, Eigen::VectorXd, double >("SimpleSplineError(BSplineDesignVariable, BSPlineExpression, )")) 22 | 23 | 24 | // ; 25 | 26 | 27 | } 28 | -------------------------------------------------------------------------------- /aslam_splines_python/src/spline_module.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace bsplines; 11 | using namespace boost::python; 12 | using namespace aslam::splines; 13 | using namespace aslam::backend; 14 | 15 | void exportBSplineMotionError(); 16 | void exportSimpleSplineError(); 17 | void exportOptBSplines(); 18 | void exportAddQuadraticIntegralExpressionErrorTerms(); 19 | 20 | template 21 | void exportBsd() 22 | { 23 | std::stringstream str; 24 | str << "BSpline" << D << "DesignVariable"; 25 | 26 | class_< BSplineDesignVariable, boost::shared_ptr< BSplineDesignVariable > >( str.str().c_str(), init() ) 27 | .def("spline", &BSplineDesignVariable::spline, return_value_policy()) 28 | .def("toExpression", &BSplineDesignVariable::toExpression) 29 | .def("numDesignVariables", &BSplineDesignVariable::numDesignVariables ) 30 | .def("designVariable", &BSplineDesignVariable::designVariable, return_internal_reference<>()) 31 | ; 32 | 33 | } 34 | 35 | 36 | 37 | BOOST_PYTHON_MODULE(libaslam_splines_python) 38 | { 39 | exportBsd<1>(); 40 | exportBsd<2>(); 41 | exportBsd<3>(); 42 | exportBsd<4>(); 43 | exportBsd<5>(); 44 | exportBsd<6>(); 45 | exportBsd<7>(); 46 | exportBsd<8>(); 47 | exportBsd<9>(); 48 | exportBsd<10>(); 49 | 50 | 51 | aslam::backend::TransformationExpression (BSplinePoseDesignVariable::*transformationAtTime1)(const aslam::backend::ScalarExpression &) = &BSplinePoseDesignVariable::transformationAtTime; 52 | aslam::backend::TransformationExpression (BSplinePoseDesignVariable::*transformationAtTime2)(const aslam::backend::ScalarExpression &, double, double) = &BSplinePoseDesignVariable::transformationAtTime; 53 | 54 | 55 | class_< BSplinePoseDesignVariable, boost::shared_ptr< BSplinePoseDesignVariable > >("BSplinePoseDesignVariable", init() ) 56 | .def("spline", &BSplinePoseDesignVariable::spline, return_value_policy()) 57 | .def("numDesignVariables", &BSplinePoseDesignVariable::numDesignVariables ) 58 | .def("designVariable", &BSplinePoseDesignVariable::designVariable, return_internal_reference<>()) 59 | .def("transformation", &BSplinePoseDesignVariable::transformation) 60 | .def("angularVelocityBodyFrame", &BSplinePoseDesignVariable::angularVelocityBodyFrame) 61 | .def("angularAccelerationBodyFrame", &BSplinePoseDesignVariable::angularAccelerationBodyFrame) 62 | .def("linearVelocity", &BSplinePoseDesignVariable::linearVelocity) 63 | .def("linearAcceleration", &BSplinePoseDesignVariable::linearAcceleration) 64 | .def("position", &BSplinePoseDesignVariable::position) 65 | .def("orientation", &BSplinePoseDesignVariable::orientation) 66 | .def("transformationAtTime", transformationAtTime1) 67 | .def("transformationAtTime", transformationAtTime2); 68 | ; 69 | 70 | 71 | class_< BSplineRSPoseDesignVariable, boost::shared_ptr< BSplineRSPoseDesignVariable > >("BSplineRSPoseDesignVariable", init() ) 72 | .def("spline", &BSplineRSPoseDesignVariable::spline, return_value_policy()) 73 | .def("numDesignVariables", &BSplineRSPoseDesignVariable::numDesignVariables ) 74 | .def("designVariable", &BSplineRSPoseDesignVariable::designVariable, return_internal_reference<>()) 75 | .def("transformation", &BSplineRSPoseDesignVariable::transformation) 76 | .def("lineDelay", &BSplineRSPoseDesignVariable::lineDelay) 77 | ; 78 | 79 | class_, bases > >("EuclideanBSplineDesignVariable", init()) 80 | .def("toEuclideanExpression", &EuclideanBSplineDesignVariable::toEuclideanExpression) 81 | .def("toEuclidean", &EuclideanBSplineDesignVariable::toEuclidean) 82 | ; 83 | 84 | exportBSplineMotionError(); 85 | exportSimpleSplineError(); 86 | exportOptBSplines(); 87 | exportAddQuadraticIntegralExpressionErrorTerms(); 88 | 89 | } 90 | -------------------------------------------------------------------------------- /aslam_splines_python/test/QuadraticIntegralError.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import aslam_backend 3 | import bsplines 4 | import aslam_splines 5 | import numpy 6 | import scipy.interpolate.fitpack as fp 7 | import scipy.integrate as integrate 8 | 9 | import unittest 10 | 11 | def createUniformKnotBSpline(order,segments,dim,knotSpacing=1.0): 12 | aspl = aslam_splines.OptEuclideanBSpline(order, dim) 13 | aspl.initConstantUniformSpline(0, segments * knotSpacing, segments, numpy.zeros((dim, 1))) 14 | kc = aspl.getNumControlVertices(); 15 | cp = numpy.random.random([dim,kc]) 16 | aspl.setControlVertices(cp) 17 | return (aspl,(aspl.getKnotsVector(),cp,order-1)) 18 | 19 | def createUniformKnotOldBSpline(order,segments,dim,knotSpacing=1.0): 20 | aspl = bsplines.BSpline(order) 21 | kr = aspl.numKnotsRequired(segments) 22 | kc = aspl.numCoefficientsRequired(segments); 23 | knots = numpy.linspace(-order,kr - 1 - order, kr)*knotSpacing 24 | cp = numpy.random.random([dim,kc]) 25 | aspl.setKnotVectorAndCoefficients(knots, cp) 26 | return aspl 27 | 28 | class TestQuadraticIntegralError(unittest.TestCase): 29 | def test_construction(self): 30 | splineOrder = 4; 31 | spline = createUniformKnotOldBSpline(splineOrder, 6, 3) 32 | sdv = aslam_splines.EuclideanBSplineDesignVariable(spline) 33 | 34 | tMin = spline.t_min(); 35 | tMax = spline.t_max(); 36 | sqrtW = numpy.matrix(numpy.random.random((3, 3))) 37 | W = sqrtW.T * sqrtW; 38 | for derivativeOrder in range(0, splineOrder - 1): 39 | p = aslam_backend.OptimizationProblem() 40 | for i in range(0, sdv.numDesignVariables()) : p.addDesignVariable(sdv.designVariable(i)) 41 | aslam_splines.addQuadraticIntegralEuclideanExpressionErrorTermsToProblem(p, tMin, tMax, 100, lambda time : sdv.toEuclideanExpression(time, derivativeOrder), sqrtW) 42 | E = 0; 43 | for i in range(0, p.numErrorTerms()): 44 | E += p.errorTerm(i).evaluateError() 45 | 46 | evalQF = lambda t: (numpy.matrix(spline.evalD(t, derivativeOrder)) * W * numpy.matrix(spline.evalD(t, derivativeOrder)).T)[0, 0]; 47 | Epy = integrate.quad(evalQF, tMin, tMax)[0] 48 | 49 | self.assertAlmostEqual(E, Epy, 3); 50 | 51 | if __name__ == '__main__': 52 | import rostest 53 | rostest.rosrun('splines', 'QuadraticIntegralError', TestQuadraticIntegralError) 54 | -------------------------------------------------------------------------------- /bsplines/.gitignore: -------------------------------------------------------------------------------- 1 | python/bsplines/libbsplines.so 2 | *.pyc 3 | /LocalBuildType.cmake 4 | -------------------------------------------------------------------------------- /bsplines/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(bsplines) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | catkin_simple(ALL_DEPS_REQUIRED) 7 | 8 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 9 | if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 10 | # deprecated-register warnings are cause by old Eigen versions 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-register") 12 | endif() 13 | 14 | # enable C++11 support 15 | if(CMAKE_VERSION VERSION_LESS "3.1") 16 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 17 | else() 18 | set(CMAKE_CXX_STANDARD 11) 19 | endif() 20 | 21 | 22 | cs_add_library(${PROJECT_NAME} 23 | src/BSpline.cpp 24 | src/BSplinePose.cpp 25 | ) 26 | 27 | if(CATKIN_ENABLE_TESTING) 28 | catkin_add_gtest(${PROJECT_NAME}_tests 29 | test/SplineTests.cpp 30 | test/BSplinePoseTests.cpp 31 | test/DiffManifoldBSplineTests.cpp 32 | test/test_main.cpp 33 | ) 34 | if(TARGET ${PROJECT_NAME}_tests) 35 | target_link_libraries(${PROJECT_NAME}_tests ${PROJECT_NAME}) 36 | endif() 37 | endif() 38 | 39 | cs_install() 40 | cs_export() 41 | -------------------------------------------------------------------------------- /bsplines/doc/footer.html: -------------------------------------------------------------------------------- 1 |
2 | Generated on $datetime for $projectname by doxygen $doxygenversion
3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /bsplines/doc/header.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | Autonomous Space Robotics Lab: $title 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 |
16 | 17 | 18 | 22 | 27 | 28 |
19 |

Autonomous Space Robotics Lab

20 |

B-Splines for State Estimation in Robotics

21 | 		23 | UTIAS 24 | ASRL 26 |
29 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/BSplinePose.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file BSpline.hpp 3 | * @author Paul Furgale 4 | * @date Fri Feb 11 13:51:57 2011 5 | * 6 | * @brief A class to facilitate state estimation for vehicles in 3D 7 | * space using B-splines. 8 | * 9 | * 10 | */ 11 | 12 | 13 | #ifndef _BSPLINE_POSE_HPP 14 | #define _BSPLINE_POSE_HPP 15 | 16 | 17 | 18 | #include "BSpline.hpp" 19 | #include 20 | 21 | namespace bsplines { 22 | 23 | /** 24 | * @class BSpline 25 | * 26 | * A class to facilitate state estimation for vehicles in 3D space using B-Splines 27 | * The spline represents a pose with respect to some navigation frame 28 | * \f$ \mathbf F_n \f$. 29 | * 30 | */ 31 | class BSplinePose : public BSpline 32 | { 33 | public: 34 | 35 | /** 36 | * Create a spline of the specified order. The resulting B-spline will 37 | * be a series of piecewise polynomials of degree splineOrder - 1. 38 | * 39 | * @param splineOrder The order of the spline. 40 | */ 41 | BSplinePose(int splineOrder, const sm::kinematics::RotationalKinematics::Ptr & rotationalKinematics); 42 | 43 | /** 44 | * A destructor. 45 | * 46 | */ 47 | ~BSplinePose(); 48 | 49 | 50 | Eigen::Matrix4d transformation(double tk) const; 51 | Eigen::Matrix4d transformationAndJacobian(double tk, Eigen::MatrixXd * J = NULL, Eigen::VectorXi * coefficientIndices = NULL) const; 52 | 53 | Eigen::Matrix4d inverseTransformationAndJacobian(double tk, Eigen::MatrixXd * J = NULL, Eigen::VectorXi * coefficientIndices = NULL) const; 54 | Eigen::Matrix4d inverseTransformation(double tk) const; 55 | 56 | 57 | Eigen::Vector4d transformVectorAndJacobian(double tk, const Eigen::Vector4d & v, Eigen::MatrixXd * J = NULL, Eigen::VectorXi * coefficientIndices = NULL) const; 58 | Eigen::Vector3d position(double tk) const; 59 | 60 | Eigen::Matrix3d orientation(double tk) const; 61 | Eigen::Matrix3d orientationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const; 62 | 63 | Eigen::Matrix3d inverseOrientation(double tk) const; 64 | Eigen::Matrix3d inverseOrientationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const; 65 | 66 | 67 | Eigen::Vector3d linearVelocity(double tk) const; 68 | Eigen::Vector3d linearVelocityBodyFrame(double tk) const; 69 | 70 | Eigen::Vector3d linearAcceleration(double tk) const; 71 | Eigen::Vector3d linearAccelerationBodyFrame(double tk) const; 72 | Eigen::Vector3d linearAccelerationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const; 73 | 74 | Eigen::Vector3d angularVelocity(double tk) const; 75 | Eigen::Vector3d angularVelocityBodyFrame(double tk) const; 76 | Eigen::Vector3d angularVelocityBodyFrameAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const; 77 | 78 | Eigen::Vector3d angularVelocityAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const; 79 | 80 | // Fabio (not used and not tested) 81 | Eigen::Vector3d angularAccelerationBodyFrame(double tk) const; 82 | Eigen::Vector3d angularAccelerationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const; 83 | Eigen::Vector3d angularAccelerationBodyFrameAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const; 84 | 85 | void initPoseSpline(double t0, double t1, const Eigen::Matrix4d & T_n_t0, const Eigen::Matrix4d & T_n_t); 86 | void initPoseSpline2(const Eigen::VectorXd & times, const Eigen::Matrix & poses, int numSegments, double lambda); 87 | void initPoseSpline3(const Eigen::VectorXd & times, const Eigen::Matrix & poses, int numSegments, double lambda); 88 | void initPoseSplineSparse(const Eigen::VectorXd & times, const Eigen::Matrix & poses, int numSegments, double lambda); 89 | void initPoseSplineSparseKnots(const Eigen::VectorXd ×, const Eigen::MatrixXd &interpolationPoints, const Eigen::VectorXd knots, double lambda); 90 | 91 | 92 | void addPoseSegment(double tk, const Eigen::Matrix4d & T_n_tk); 93 | void addPoseSegment2(double tk, const Eigen::Matrix4d & T_n_tk, double lambda); 94 | 95 | Eigen::Matrix4d curveValueToTransformation( const Eigen::VectorXd & c ) const; 96 | Eigen::VectorXd transformationToCurveValue( const Eigen::Matrix4d & T ) const; 97 | 98 | sm::kinematics::RotationalKinematics::Ptr rotation() const; 99 | 100 | Eigen::Matrix4d curveValueToTransformationAndJacobian( const Eigen::VectorXd & c, Eigen::MatrixXd * J ) const; 101 | 102 | private: 103 | sm::kinematics::RotationalKinematics::Ptr rotation_; 104 | }; 105 | 106 | } 107 | 108 | 109 | #endif /* _BSPLINE_POSE_HPP */ 110 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/DynamicOrTemplateInt.hpp: -------------------------------------------------------------------------------- 1 | #ifndef DYNAMICORTEMPLATEINT_HPP_ 2 | #define DYNAMICORTEMPLATEINT_HPP_ 3 | #include 4 | #include 5 | 6 | namespace eigenTools { 7 | struct DynamicOrTemplateIntBase { 8 | virtual inline int getValue() const { return -2; }; 9 | virtual ~DynamicOrTemplateIntBase(){}; 10 | 11 | inline static bool isDynamic() { 12 | return false; 13 | } 14 | }; 15 | 16 | template 17 | struct DynamicOrTemplateInt : public DynamicOrTemplateIntBase { 18 | enum { VALUE = VALUE_T, IS_DYNAMIC = 0 }; 19 | 20 | DynamicOrTemplateInt(int value = VALUE_T){ 21 | SM_ASSERT_EQ(std::runtime_error, value, (int)VALUE, "Please don't set dynamic parameter value with fixed size template argument to something else!") 22 | } 23 | 24 | virtual inline int getValue() const { 25 | return VALUE; 26 | } 27 | 28 | inline operator int() const { 29 | return getValue(); 30 | } 31 | }; 32 | 33 | template <> 34 | struct DynamicOrTemplateInt{ 35 | enum { VALUE = Eigen::Dynamic, IS_DYNAMIC = 1 }; 36 | int _value; 37 | 38 | DynamicOrTemplateInt(int value) : _value(value){ 39 | if(value == Eigen::Dynamic) 40 | SM_ASSERT_NE(std::runtime_error, value, Eigen::Dynamic, "Please set the dynamic parameter in the constructor when you use the Eigen::Dynamic template argument!") 41 | } 42 | 43 | inline int getValue() const { 44 | return _value; 45 | } 46 | 47 | inline bool isDynamic() const { 48 | return true; 49 | } 50 | 51 | inline operator int() const { 52 | return getValue(); 53 | } 54 | }; 55 | 56 | #define multiplyEigenSize(a, b) (a == Eigen::Dynamic || b == Eigen::Dynamic? Eigen ::Dynamic : a * b) 57 | 58 | template 59 | inline DynamicOrTemplateInt operator * (const DynamicOrTemplateInt & a, const DynamicOrTemplateInt & b){ 60 | return DynamicOrTemplateInt(multiplyEigenSize(a.getValue(), b.getValue())); 61 | } 62 | 63 | template struct DynOrStaticSizedArray { 64 | T p[ISize]; 65 | DynOrStaticSizedArray(int size){ 66 | if(size != ISize) throw new std::runtime_error("dynamic and static size have to match unless the static size is DYNAMIC!"); 67 | } 68 | 69 | inline T & operator [] (int i){ 70 | SM_ASSERT_GE_LT_DBG(std::runtime_error, i, 0, ISize, "index out of bounds"); 71 | return p[i]; 72 | } 73 | 74 | inline const T & operator [] (int i) const { 75 | SM_ASSERT_GE_LT_DBG(std::runtime_error, i, 0, ISize, "index out of bounds"); 76 | return p[i]; 77 | } 78 | }; 79 | 80 | template struct DynOrStaticSizedArray { 81 | T * p; 82 | DynOrStaticSizedArray(int size) : p(new T[size]){ 83 | if(size < 0) throw new std::runtime_error("dynamic size has to be non negative"); 84 | } 85 | ~ DynOrStaticSizedArray(){ 86 | delete[] p; 87 | } 88 | 89 | inline T & operator [] (int i){ 90 | SM_ASSERT_GE_DBG(std::runtime_error, i, 0, "index out of bounds"); 91 | return p[i]; 92 | } 93 | 94 | inline const T & operator [] (int i) const { 95 | SM_ASSERT_GE_DBG(std::runtime_error, i, 0, "index out of bounds"); 96 | return p[i]; 97 | } 98 | 99 | }; 100 | } 101 | 102 | 103 | #endif /* DYNAMICORTEMPLATEINT_HPP_ */ 104 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/EuclideanBSpline.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * EuclideanBSpline.hpp 3 | * 4 | * Created on: May 10, 2012 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef FLATBSPLINE_HPP_ 9 | #define FLATBSPLINE_HPP_ 10 | 11 | #include "manifolds/EuclideanSpace.hpp" 12 | #include "DiffManifoldBSpline.hpp" 13 | 14 | namespace bsplines { 15 | template 16 | struct EuclideanBSplineConfiguration : public DiffManifoldBSplineConfiguration { 17 | public: 18 | typedef TDiffManifoldConfiguration ManifoldConf; 19 | typedef DiffManifoldBSplineConfiguration ParentConf; 20 | typedef EuclideanBSplineConfiguration Conf; 21 | typedef DiffManifoldBSpline BSpline; 22 | 23 | #if __cplusplus >= 201103L 24 | using ParentConf::ParentConf; 25 | #else 26 | EuclideanBSplineConfiguration(TDiffManifoldConfiguration manifoldConfiguration = TDiffManifoldConfiguration(), int splineOrder = ISplineOrder) : ParentConf(manifoldConfiguration, splineOrder) {} 27 | #endif 28 | }; 29 | 30 | template 31 | class DiffManifoldBSpline, TConfigurationDerived> : public DiffManifoldBSpline::ParentConf, TConfigurationDerived> { 32 | public: 33 | typedef DiffManifoldBSpline, TConfigurationDerived> this_t; 34 | typedef DiffManifoldBSpline::ParentConf, TConfigurationDerived> parent_t; 35 | typedef typename parent_t::configuration_t configuration_t; 36 | typedef typename parent_t::spline_t spline_t; 37 | typedef typename parent_t::manifold_t manifold_t; 38 | typedef typename parent_t::time_t time_t; 39 | typedef typename parent_t::point_t point_t; 40 | typedef typename parent_t::SplineOrderVector SplineOrderVector; 41 | typedef typename parent_t::SegmentMapConstIterator SegmentMapConstIterator; 42 | typedef typename parent_t::full_jacobian_t full_jacobian_t; 43 | 44 | DiffManifoldBSpline(int splineOrder = parent_t::SplineOrder, int dimension = parent_t::Dimension) : parent_t(configuration_t (typename configuration_t::ManifoldConf(dimension), splineOrder)){} 45 | DiffManifoldBSpline(configuration_t conf) : parent_t(conf){} 46 | 47 | point_t evalIntegral(const time_t & t1, const time_t & t2) const; 48 | 49 | template 50 | class Evaluator : public parent_t::template Evaluator { 51 | public : 52 | Evaluator(const spline_t & spline, const time_t & t); 53 | 54 | point_t eval() const; 55 | 56 | point_t evalD(int derivativeOrder) const; 57 | 58 | void evalJacobian(int derivativeOrder, full_jacobian_t & jacobian) const; 59 | 60 | //friends: 61 | template friend class DiffManifoldBSpline; 62 | FRIEND_TEST(EuclideanBSplineTestSuite, testEuclideanDiffManifoldBSplineBehavesAsVectorSpaceBSpline); 63 | }; 64 | 65 | template 66 | inline Evaluator getEvaluatorAt(const time_t & t) const { return parent_t::template getEvaluatorAt(t); } 67 | 68 | protected: 69 | enum IteratorPosition { IteratorPosition_first, IteratorPosition_last, IteratorPosition_end }; 70 | enum AddOrSet { AddOrSet_add, AddOrSet_set }; 71 | 72 | template 73 | inline static void stepIterator(TIterator & it); 74 | 75 | template 76 | inline static void computeControlVertexSequenceLinearCombinationInto(const SegmentMapConstIterator & it, const TCoefficientVector & coefficients, point_t & result); 77 | 78 | template friend class Evaluator; 79 | }; 80 | 81 | 82 | template 83 | 84 | #if __cplusplus >= 201103L 85 | using EuclideanBSpline = typename EuclideanBSplineConfiguration, ISplineOrder, TTimePolicy>::BSpline; 86 | #else 87 | class EuclideanBSpline { 88 | public: 89 | typedef EuclideanBSplineConfiguration, ISplineOrder, TTimePolicy> CONF; 90 | typedef typename CONF::BSpline TYPE; 91 | }; 92 | #endif 93 | } 94 | 95 | #include "implementation/EuclideanBSplineImpl.hpp" 96 | 97 | #endif /* FLATBSPLINE_HPP_ */ 98 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/ExternalIncludes.hpp: -------------------------------------------------------------------------------- 1 | #ifndef EXTERNALINCLUDE_HPP_ 2 | #define EXTERNALINCLUDE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/NodeDistributedCache.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * NodeDistributedCache.hpp 3 | * 4 | * Created on: Oct 4, 2012 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef NODEDISTRIBUTEDCACHE_HPP_ 9 | #define NODEDISTRIBUTEDCACHE_HPP_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace nodecache { 17 | 18 | template 19 | struct NodeCacheAccessor; 20 | 21 | template 22 | class CacheInvalidator{ 23 | virtual ~CacheInvalidator() = 0; 24 | void virtual invalidate(TNode & node, NodeCacheAccessor & nodeCacheAccessor) = 0; 25 | }; 26 | 27 | 28 | template 29 | class NodeDistributedCache{ 30 | public: 31 | template 32 | class NodeCacheSlot; 33 | 34 | class PerNodeCache{ 35 | public: 36 | class PerNodeCacheEntry{ 37 | public: 38 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 39 | void setValid(bool valid); 40 | bool isValid(); 41 | }; 42 | 43 | template 44 | class PerNodeCacheValue : public PerNodeCacheEntry { 45 | TValue _value; 46 | bool _valid; 47 | PerNodeCacheValue() : _valid(false){ 48 | } 49 | friend class NodeCacheSlot; 50 | public: 51 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 52 | 53 | inline TValue & accessValue(){ 54 | return _value; 55 | } 56 | 57 | template 58 | inline const TValue & get(TFunctor & updater){ 59 | if(!isValid()){ 60 | updater(_value); 61 | setValid(true); 62 | } 63 | return _value; 64 | } 65 | 66 | inline operator TValue & (){ 67 | return _value; 68 | } 69 | 70 | inline void setValid(bool valid){ _valid = valid; } 71 | inline bool isValid(){ return _valid; } 72 | }; 73 | 74 | 75 | void handleUpdatedValueEvent(){ 76 | for(typename std::vector::iterator i = entries.begin(), end = entries.end(); i != end; i++){ 77 | 78 | } 79 | } 80 | 81 | ~PerNodeCache(){ 82 | for(typename std::vector::iterator i = entries.begin(), end = entries.end(); i != end; i++){ 83 | if(*i!=NULL) 84 | delete *i; 85 | } 86 | } 87 | private: 88 | std::vector entries; 89 | 90 | template friend class PerNodeCacheValue; 91 | template friend class NodeCacheSlot; 92 | 93 | public: //TODO This public should not be needed thanks to the friend declaration above. But clang++-3.8 disagrees! 94 | PerNodeCacheEntry * & getEntry(size_t index){ 95 | if(index >= entries.size()){ 96 | entries.resize(index+1); 97 | } 98 | return entries[index]; 99 | } 100 | }; 101 | 102 | template 103 | class NodeCacheSlot { 104 | size_t _index; 105 | NodeDistributedCache & _cache; 106 | 107 | NodeCacheSlot(size_t index, NodeDistributedCache & cache) : _index(index), _cache(cache) {} 108 | 109 | friend class NodeDistributedCache; 110 | 111 | public: 112 | ~NodeCacheSlot(){ 113 | _cache.freeIndex(_index); 114 | } 115 | typedef typename PerNodeCache::template PerNodeCacheValue CacheValueT; 116 | inline TValue & accessValue(TNode & node){ 117 | return accessNodeCache(node).accessValue(); 118 | } 119 | 120 | inline CacheValueT & accessNodeCache(TNode & node){ 121 | typename PerNodeCache::PerNodeCacheEntry * & pEntry = NodeCacheAccessor::accessNodeCache(node).getEntry(_index); 122 | if(pEntry == NULL){ 123 | pEntry = new CacheValueT(); 124 | } 125 | return *static_cast(pEntry); 126 | } 127 | }; 128 | 129 | template 130 | inline std::shared_ptr > registerCacheableValue(){ 131 | return std::shared_ptr >(new NodeCacheSlot(getNextFreeIndex(), *this)); 132 | } 133 | private: 134 | std::vector slotUsage; 135 | int getNextFreeIndex(){ 136 | //TODO optimize : optimize for continuous usage e.g. by introducing a full up to index 137 | int j = 0; 138 | for(std::vector::iterator i = slotUsage.begin(), end = slotUsage.end(); i != end; (i++, j++)){ 139 | if(!*i){ 140 | *i = true; 141 | return j; 142 | } 143 | } 144 | slotUsage.push_back(true); 145 | return slotUsage.size() - 1; 146 | }; 147 | 148 | void freeIndex(int i){ 149 | SM_ASSERT_TRUE_DBG(std::runtime_error, slotUsage[i], "bug in NodeCacheAccessor!"); 150 | slotUsage[i] = false; 151 | } 152 | public: 153 | ~NodeDistributedCache(){ 154 | for(std::vector::iterator i = slotUsage.begin(), end = slotUsage.end(); i != end; i++){ 155 | SM_ASSERT_FALSE(std::runtime_error, *i, "memory leak : there are still some slots alive when deleting the entire cache!"); 156 | } 157 | } 158 | }; 159 | 160 | template 161 | struct NodeCacheAccessor { 162 | static inline typename NodeDistributedCache::PerNodeCache & accessNodeCache(TNode & node){ return node.accessCache();} 163 | }; 164 | 165 | } 166 | 167 | #endif /* NODEDISTRIBUTEDCACHE_HPP_ */ 168 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/NsecTimePolicy.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * NsecTimePolicy.hpp 3 | * 4 | * Created on: Sep 16, 2013 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef NSECTIMEPOLICY_HPP_ 9 | #define NSECTIMEPOLICY_HPP_ 10 | 11 | #include 12 | #include "SimpleTypeTimePolicy.hpp" 13 | 14 | namespace bsplines { 15 | 16 | struct NsecTimePolicy : public SimpleTypeTimePolicy { 17 | constexpr inline static sm::timing::NsecTime getOne() { 18 | return sm::timing::NsecTime(1E9); 19 | } 20 | }; 21 | 22 | } // namespace bsplines 23 | 24 | #endif /* NSECTIMEPOLICY_HPP_ */ 25 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/SimpleTypeTimePolicy.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SimpleTypeTimePolicy.hpp 3 | * 4 | * Created on: May 10, 2012 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef SIMPLETYPETIMEPOLICY_HPP_ 9 | #define SIMPLETYPETIMEPOLICY_HPP_ 10 | 11 | #include 12 | #include 13 | 14 | namespace bsplines { 15 | template 16 | struct SimpleTypeTimePolicy { 17 | typedef SimpleType_ time_t; 18 | typedef SimpleType_ duration_t; 19 | 20 | inline static duration_t computeDuration(time_t from, time_t till){ 21 | return till - from; 22 | } 23 | 24 | inline static duration_t addScaledDuration(time_t from, duration_t dist, int scale){ 25 | return from + dist * scale; 26 | } 27 | 28 | inline static double divideDurations(duration_t a, duration_t b){ 29 | return (double) a/b; 30 | } 31 | 32 | inline static time_t linearlyInterpolate(time_t from, time_t till, int segments, int pos) 33 | { 34 | if(pos == segments) return till; 35 | return from + (computeDuration(from, till) * pos) / segments; 36 | } 37 | 38 | inline static int getSegmentNumber(time_t from, time_t till, int segments, time_t t) 39 | { 40 | duration_t duration = computeDuration(from, till); 41 | assert(duration * segments / duration == segments); 42 | // The above assertion fails if the type time_t is not capable to carry out the following operation precise enough for the duration given by (from,till). I.e. to correctly invert the linearlyInterpolate method above. 43 | // This basically means the spline is too large (in duration or resolution) to be used with this time policy. So either shorten it or implement a better time policy. 44 | // For example with nanoseconds as signed 64 bit integers this holds if 45 | // duration * segments = duration * duration / T <= ~9.22e9 seconds. // where T is the uniform knot distance. 46 | // I.e. for 1000s the maximum supported knot rate is around 9kHz. 47 | // TODO increase the maximal supported duration^2 * knotFreq. 48 | return ((t - from) * segments) / duration; 49 | } 50 | 51 | constexpr inline static duration_t getZero(){ 52 | return duration_t(0.0); 53 | } 54 | 55 | constexpr inline static duration_t getOne(){ 56 | static_assert(!std::is_integral::value, "Please shadow this (getOne) method for your simple type's time policy - preferably with an inline or even better constexpr function."); 57 | return duration_t(1.0); 58 | } 59 | }; 60 | 61 | } // namespace bsplines 62 | 63 | #endif /* SIMPLETYPETIMEPOLICY_HPP_ */ 64 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/UnitQuaternionBSpline.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * UnitQuaternionBSpline.hpp 3 | * 4 | * Created on: May 10, 2012 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef UNITQUATERNIONBSPLINE_HPP_ 9 | #define UNITQUATERNIONBSPLINE_HPP_ 10 | 11 | #include "manifolds/UnitQuaternionManifold.hpp" 12 | #include "DiffManifoldBSpline.hpp" 13 | 14 | namespace bsplines { 15 | template , int ISplineOrder = Eigen::Dynamic, typename TTimePolicy = DefaultTimePolicy> 16 | struct UnitQuaternionBSplineConfiguration : public DiffManifoldBSplineConfiguration { 17 | public: 18 | typedef DiffManifoldBSplineConfiguration ParentConf; 19 | typedef UnitQuaternionBSplineConfiguration Conf; 20 | typedef TDiffManifoldConfiguration ManifoldConf; 21 | typedef DiffManifoldBSpline BSpline; 22 | 23 | UnitQuaternionBSplineConfiguration(TDiffManifoldConfiguration manifoldConfiguration = TDiffManifoldConfiguration(), int splineOrder = ISplineOrder) : ParentConf(manifoldConfiguration, splineOrder) {} 24 | UnitQuaternionBSplineConfiguration(int splineOrder) : ParentConf(TDiffManifoldConfiguration(), splineOrder) {} 25 | }; 26 | 27 | namespace internal { 28 | template 29 | struct DiffManifoldBSplineTraits > { 30 | enum { NeedsCumulativeBasisMatrices = true }; 31 | }; 32 | } 33 | 34 | template 35 | class DiffManifoldBSpline, TConfigurationDerived> : public DiffManifoldBSpline::ParentConf, TConfigurationDerived> { 36 | typedef DiffManifoldBSpline::ParentConf, TConfigurationDerived> parent_t; 37 | public: 38 | typedef typename parent_t::configuration_t configuration_t; 39 | typedef typename parent_t::spline_t spline_t; 40 | typedef typename parent_t::manifold_t manifold_t; 41 | typedef typename parent_t::time_t time_t; 42 | typedef typename parent_t::point_t point_t; 43 | typedef typename parent_t::tangent_vector_t tangent_vector_t; 44 | typedef typename parent_t::dmatrix_t dmatrix_t; 45 | typedef typename parent_t::full_jacobian_t full_jacobian_t; 46 | typedef typename parent_t::SplineOrderVector SplineOrderVector; 47 | typedef typename parent_t::SegmentMapConstIterator SegmentMapConstIterator; 48 | typedef Eigen::Matrix angular_jacobian_t; 49 | 50 | SM_DEFINE_EXCEPTION(Exception, std::runtime_error); 51 | 52 | enum { 53 | MaxSupportedDerivativeOrderEvaluation = 3, 54 | MaxSupportedDerivativeOrderJacobian = 2 55 | }; 56 | 57 | DiffManifoldBSpline(int splineOrder = parent_t::SplineOrder) : parent_t(UnitQuaternionBSplineConfiguration(typename configuration_t::ManifoldConf(), splineOrder)){} 58 | DiffManifoldBSpline(const configuration_t & conf) : parent_t(conf){} 59 | 60 | public: 61 | template 62 | class Evaluator : public parent_t::template Evaluator { 63 | template friend struct SplineEvalRiDTester; 64 | template friend struct SplineEvalRiDJacobianTester; 65 | template friend struct AngularDerivativeJacobianEvaluator; 66 | 67 | public : 68 | Evaluator(const spline_t & spline, const time_t & t); 69 | 70 | point_t evalD(int derivativeOrder) const; 71 | 72 | point_t evalD1Special() const; 73 | 74 | point_t evalDRecursive(int derivativeOrder) const; 75 | 76 | tangent_vector_t evalAngularVelocity() const; 77 | 78 | tangent_vector_t evalAngularAcceleration() const; 79 | 80 | void evalJacobian(int derivativeOrder, full_jacobian_t & jacobian) const; 81 | 82 | void evalJacobianDRecursive(int derivativeOrder, full_jacobian_t & jacobian) const; 83 | 84 | void evalJacobian(full_jacobian_t & jacobian) const; 85 | 86 | void evalAngularVelocityJacobian(angular_jacobian_t & jacobian) const; 87 | 88 | void evalAngularAccelerationJacobian(angular_jacobian_t & jacobian) const; 89 | 90 | template 91 | tangent_vector_t evalAngularDerivative() const; 92 | 93 | template 94 | void evalAngularDerivativeJacobian(angular_jacobian_t & jacobian) const; 95 | private: 96 | struct CalculationCache; 97 | inline int getNumVectors() const; 98 | 99 | point_t evalRiD(const CalculationCache & cache, int derivativeOrder, int i) const; 100 | 101 | point_t evalDRecursiveProductRest(const CalculationCache & cache, int derivativeOrder, int i, const int kombinatorialFactor) const; 102 | 103 | point_t getRiQuaternionProduct(const CalculationCache & cache, int from, int to) const; 104 | 105 | Eigen::Matrix getPhiVectorJacobian(const CalculationCache & cache, int i) const; 106 | 107 | static bool isJacobianZero(int i, int j); 108 | 109 | static double getJacobianSignum(int i, int j); 110 | 111 | dmatrix_t getRiDJacobian(const CalculationCache & cache, int derivativeOrder, int i, int j) const; 112 | 113 | dmatrix_t getRiDJacobian(const CalculationCache & cache, int derivativeOrder, int i) const; 114 | dmatrix_t evalJacobianDRecursiveProductRest(const CalculationCache & cache, int derivativeOrder, int i, const int kombinatorialFactor, int j) const; 115 | }; 116 | 117 | template 118 | inline Evaluator getEvaluatorAt(const time_t & t) const { return parent_t::template getEvaluatorAt(t); } 119 | }; 120 | 121 | 122 | template 123 | #if __cplusplus >= 201103L 124 | using UnitQuaternionBSpline = typename UnitQuaternionBSplineConfiguration, ISplineOrder, TTimePolicy>::BSpline; 125 | #else 126 | class UnitQuaternionBSpline { 127 | public: 128 | typedef UnitQuaternionBSplineConfiguration, ISplineOrder, TTimePolicy> CONF; 129 | typedef typename CONF::BSpline TYPE; 130 | }; 131 | #endif 132 | } 133 | 134 | #include "implementation/UnitQuaternionBSplineImpl.hpp" 135 | #endif /* UNITQUATERNIONBSPLINE_HPP_ */ 136 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/implementation/DiffManifoldBSplineTools.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * DiffManifoldBSplineTools.hpp 3 | * 4 | * Created on: Aug 27, 2013 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef DIFFMANIFOLDBSPLINETOOLS_HPP_ 9 | #define DIFFMANIFOLDBSPLINETOOLS_HPP_ 10 | 11 | 12 | namespace bsplines { 13 | namespace internal{ 14 | 15 | template 16 | inline int moveIterator(Iterator & it, const Iterator & limit, int steps) 17 | { 18 | if(steps == 0) 19 | return 0; 20 | int count = 0; 21 | if(steps > 0){ 22 | for (int c = steps; c>0; c--){ 23 | if(it == limit) 24 | break; 25 | it++; 26 | count ++; 27 | } 28 | } 29 | else{ 30 | for (int c = -steps; c>0; c--){ 31 | if(it == limit) 32 | break; 33 | it--; 34 | count --; 35 | } 36 | } 37 | 38 | return count; 39 | } 40 | 41 | template 42 | inline Iterator getMovedIterator(const Iterator & it, const Iterator & limit, int steps) 43 | { 44 | Iterator nIt(it); 45 | moveIterator(nIt, limit, steps); 46 | return nIt; 47 | } 48 | 49 | } 50 | } 51 | 52 | #endif /* DIFFMANIFOLDBSPLINETOOLS_HPP_ */ 53 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/implementation/EuclideanBSplineImpl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * EuclideanBSpline.hpp 3 | * 4 | * Created on: May 10, 2012 5 | * Author: hannes 6 | */ 7 | 8 | #include "bsplines/EuclideanBSpline.hpp" 9 | 10 | namespace bsplines { 11 | #define _TEMPLATE template 12 | #define _CLASS DiffManifoldBSpline, TConfigurationDerived> 13 | 14 | _TEMPLATE 15 | template 16 | inline void _CLASS::stepIterator (TIterator & it){ 17 | if(EPosition == IteratorPosition_first) it++; 18 | else it--; 19 | } 20 | 21 | _TEMPLATE 22 | template 23 | inline void _CLASS::computeControlVertexSequenceLinearCombinationInto(const SegmentMapConstIterator & it, const TCoefficientVector & coefficients, point_t & result) { 24 | SegmentMapConstIterator lIt = it; 25 | 26 | if(EPosition == IteratorPosition_end){ 27 | lIt --; 28 | } 29 | 30 | for( 31 | int 32 | start = EPosition == IteratorPosition_first ? 0 : coefficients.size() - 1, 33 | i = start, 34 | end = EPosition == IteratorPosition_first ? coefficients.size() : -1; 35 | i != end ; 36 | stepIterator(i) 37 | ){ 38 | if(EAddOrSet == AddOrSet_add || i != start){ 39 | result += coefficients[i] * lIt->second.getControlVertex(); 40 | } 41 | else result = coefficients[i] * lIt->second.getControlVertex(); 42 | stepIterator(lIt); 43 | } 44 | } 45 | 46 | _TEMPLATE 47 | typename _CLASS::point_t _CLASS::evalIntegral(const time_t & t1, const time_t & t2) const 48 | { 49 | if(t1 > t2) return -evalIntegral(t2, t1); 50 | 51 | point_t integral = point_t::Zero((int)this->getDimension()); 52 | if(t1 == t2) return integral; 53 | 54 | const int splineOrder = this->getSplineOrder(); 55 | 56 | Evaluator<0> eval1 = this->getEvaluatorAt<0>(t1); 57 | 58 | // LHS remainder. 59 | SplineOrderVector v(splineOrder); 60 | double segmentLength = this->getDurationAsDouble(eval1.getSegmentLength()); 61 | if(segmentLength > 1e-16 && eval1.getRelativePositionInSegment() > 1e-16){ 62 | eval1.computeLocalViInto(v); 63 | computeControlVertexSequenceLinearCombinationInto(eval1.getFirstRelevantSegmentIterator(), (-segmentLength) * v, integral); 64 | } 65 | 66 | // central time segments. 67 | for(int i = 0; i < splineOrder; i++) v(i) = 1.0/(i + 1.0); 68 | 69 | auto it = eval1.getLastRelevantSegmentIterator(); 70 | Evaluator<0> eval2 = this->getEvaluatorAt<0>(t2); 71 | const auto end = eval2.getLastRelevantSegmentIterator(); 72 | 73 | SplineOrderVector localVi(splineOrder); 74 | for(; it != end; it++) 75 | { 76 | this->computeLocalViInto(v, localVi, it); 77 | computeControlVertexSequenceLinearCombinationInto(it, this->getDurationAsDouble(this->computeSegmentLength(it)) * localVi, integral); 78 | } 79 | 80 | // RHS remainder. 81 | segmentLength = this->getDurationAsDouble(eval2.getSegmentLength()); 82 | if(segmentLength > 1e-16 && eval2.getRelativePositionInSegment() > 1e-16){ 83 | eval2.computeLocalViInto(v); 84 | computeControlVertexSequenceLinearCombinationInto(eval2.getFirstRelevantSegmentIterator(), segmentLength * v, integral); 85 | } 86 | return integral; 87 | } 88 | 89 | _TEMPLATE 90 | template 91 | _CLASS::Evaluator::Evaluator(const spline_t & spline, const time_t & t) : parent_t::template Evaluator (spline, t) 92 | { 93 | } 94 | 95 | _TEMPLATE 96 | template 97 | inline typename _CLASS::point_t _CLASS::Evaluator::eval() const 98 | { 99 | return evalD(0); 100 | } 101 | 102 | _TEMPLATE 103 | template 104 | typename _CLASS::point_t _CLASS::Evaluator::evalD(int derivativeOrder) const 105 | { 106 | SM_ASSERT_GE(typename parent_t::Exception, derivativeOrder, 0, "To integrate, use the integral function"); 107 | if(IMaximalDerivativeOrder != Eigen::Dynamic){ 108 | SM_ASSERT_LT(typename parent_t::Exception, derivativeOrder, IMaximalDerivativeOrder + 1, "only derivatives up to the evaluator's template argument IMaximalDerivativeOrder are allowed"); 109 | } 110 | 111 | point_t rv((int)this->_spline.getDimension()); 112 | if(derivativeOrder < this->_spline.getSplineOrder()) 113 | computeControlVertexSequenceLinearCombinationInto(this->getLastRelevantSegmentIterator(), this->getLocalBi(derivativeOrder), rv); 114 | else rv.setZero(); 115 | return rv; 116 | } 117 | 118 | _TEMPLATE 119 | template 120 | void _CLASS::Evaluator::evalJacobian(int derivativeOrder, full_jacobian_t & jacobian) const 121 | { 122 | SM_ASSERT_GE(typename parent_t::Exception, derivativeOrder, 0, "To integrate, use the integral function"); 123 | if(IMaximalDerivativeOrder != Eigen::Dynamic){ 124 | SM_ASSERT_LT(typename parent_t::Exception, derivativeOrder, IMaximalDerivativeOrder + 1, "only derivatives up to the evaluator's template argument IMaximalDerivativeOrder are allowed"); 125 | } 126 | // The Jacobian 127 | const int D = this->_spline.getDimension(), splineOrder = this->_spline.getSplineOrder(); 128 | if(jacobian.rows() != D || jacobian.cols() != D * splineOrder) jacobian.resize(D, D * splineOrder); 129 | const SplineOrderVector & u = this->getLocalBi(derivativeOrder); 130 | 131 | const auto id = (Eigen::Matrix::Identity(D, D)); 132 | for(int i = 0; i < splineOrder; i++) 133 | { 134 | jacobian.block(0, i*D, D, D) = id * u[i]; 135 | } 136 | } 137 | 138 | #undef _CLASS 139 | #undef _TEMPLATE 140 | } 141 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/manifolds/DiffManifold.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Manifold.hpp 3 | * 4 | * Created on: 28.07.2012 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef MANIFOLD_HPP_ 9 | #define MANIFOLD_HPP_ 10 | 11 | #include "bsplines/DynamicOrTemplateInt.hpp" 12 | namespace manifolds { 13 | enum class CanonicalConnection { LEFT, RIGHT }; 14 | 15 | template 16 | class DiffManifold : public DiffManifold{ 17 | typedef DiffManifold parent_t; 18 | protected: 19 | DiffManifold(const TConfigurationDerived & conf) : parent_t(conf) {} 20 | }; 21 | 22 | class DiffManifoldConfigurationBase { 23 | }; 24 | 25 | template 26 | struct DiffManifoldConfiguration : public DiffManifoldConfigurationBase { 27 | typedef DiffManifoldConfigurationBase ParentConf; 28 | typedef DiffManifoldConfiguration Conf; 29 | 30 | typedef eigenTools::DynamicOrTemplateInt Dimension; 31 | typedef eigenTools::DynamicOrTemplateInt PointSize; 32 | 33 | Dimension getDimension() const; 34 | PointSize getPointSize() const; 35 | 36 | typedef DiffManifold Manifold; 37 | typedef TScalar scalar_t; 38 | }; 39 | 40 | namespace internal { 41 | template 42 | struct DiffManifoldConfigurationData : public TConfiguration { 43 | }; 44 | 45 | template 46 | struct DiffManifoldConfigurationTypeTrait { 47 | enum { Dimension = TConfiguration::Dimension::VALUE, PointSize = TConfiguration::PointSize::VALUE}; 48 | 49 | typedef TConfiguration configuration_t; 50 | typedef typename TConfiguration::Manifold Manifold; 51 | typedef typename TConfiguration::scalar_t scalar_t; 52 | typedef Eigen::Matrix point_t; 53 | typedef Eigen::Matrix tangent_vector_t; 54 | typedef Eigen::Matrix dmatrix_t; 55 | typedef Eigen::Matrix dmatrix_transposed_t; 56 | typedef Eigen::Matrix dmatrix_point2point_t; 57 | }; 58 | 59 | template 60 | struct DiffManifoldPointUpdateTraits : public DiffManifoldPointUpdateTraits { 61 | }; 62 | 63 | template 64 | struct DiffManifoldPointUpdateTraits { 65 | typedef DiffManifoldConfigurationTypeTrait Types; 66 | typedef typename Types::point_t point_t; 67 | typedef typename Types::tangent_vector_t tangent_vector_t; 68 | typedef typename Types::Manifold Manifold; 69 | inline static void update(const Manifold & /*manifold*/, point_t & /*point*/, const tangent_vector_t & /*vec*/){ 70 | SM_THROW(std::runtime_error, NotImplementedMessage); 71 | } 72 | inline static void minimalDifference(const Manifold &, const Eigen::MatrixXd& /*xHat*/, const typename Manifold::point_t & /*to*/, Eigen::VectorXd& /*outDifference*/) { 73 | SM_THROW(std::runtime_error, NotImplementedMessage); 74 | }; 75 | inline static void minimalDifferenceAndJacobian(const Manifold &, const Eigen::MatrixXd& /*xHat*/, const typename Manifold::point_t & /*to*/, Eigen::VectorXd& /*outDifference*/, Eigen::MatrixXd& /*outJacobian*/) { 76 | SM_THROW(std::runtime_error, NotImplementedMessage); 77 | }; 78 | private: 79 | constexpr static const char * NotImplementedMessage = "Point updates / minimal differences not implemented for this manifold. Pleas specialize manifolds::internal::DiffManifoldPointUpdateTraits"; 80 | }; 81 | } 82 | 83 | template 84 | class DiffManifold { 85 | public: 86 | typedef internal::DiffManifoldConfigurationTypeTrait Types; 87 | enum { Dimension = Types::Dimension, PointSize = Types::PointSize}; 88 | typedef typename Types::configuration_t configuration_t; 89 | typedef typename Types::scalar_t scalar_t; 90 | typedef typename Types::point_t point_t; 91 | typedef typename Types::tangent_vector_t tangent_vector_t; 92 | typedef typename Types::dmatrix_t dmatrix_t; 93 | typedef typename Types::dmatrix_transposed_t dmatrix_transposed_t; 94 | typedef typename Types::dmatrix_point2point_t dmatrix_point2point_t; 95 | 96 | inline DiffManifold(const TConfigurationDerived & configuration) : _configuration(configuration) {} 97 | 98 | inline configuration_t getConfiguration() const { return _configuration; } 99 | inline typename configuration_t::Dimension getDimension() const { return _configuration.getDimension(); }; 100 | inline typename configuration_t::PointSize getPointSize() const { return _configuration.getPointSize(); }; 101 | 102 | point_t getDefaultPoint() const; 103 | 104 | static inline void scaleVectorInPlace(tangent_vector_t & vec, const scalar_t scalar){ vec *= scalar; } 105 | static inline void scaleVectorInto(const tangent_vector_t & vec, const scalar_t scalar, tangent_vector_t & result){ result = vec * scalar;} 106 | static inline tangent_vector_t scaleVector(const tangent_vector_t & vec, const scalar_t scalar){ return vec * scalar; } 107 | 108 | bool isInManifold(const point_t & /* pt */) const { return true; } 109 | void projectIntoManifold(point_t & /* pt */) const { } 110 | void randomizePoint(point_t & pt) const { pt = point_t::random(getPointSize()); } 111 | point_t getRandomPoint() const { point_t p((int)getPointSize()); getDerived().randomizePoint(p); return p; } 112 | protected: 113 | TConfigurationDerived _configuration; 114 | typedef DiffManifold DERIVED; 115 | inline DERIVED & getDerived() { return static_cast(*this); } 116 | inline const DERIVED & getDerived() const { return static_cast(*this); } 117 | }; 118 | } 119 | 120 | #endif /* MANIFOLD_HPP_ */ 121 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/manifolds/EuclideanSpace.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * EuclideanSpace.hpp 3 | * 4 | * Created on: Jul 26, 2012 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef FLATRIEMANNIANGEOMETRY_HPP_ 9 | #define FLATRIEMANNIANGEOMETRY_HPP_ 10 | 11 | #include "LieGroup.hpp" 12 | 13 | namespace manifolds { 14 | 15 | template 16 | struct EuclideanSpaceConf : public LieGroupConf { 17 | typedef LieGroupConf ParentConf; 18 | typedef DiffManifold ParentManifold; 19 | typedef EuclideanSpaceConf Conf; 20 | typedef DiffManifold Manifold; 21 | 22 | typename ParentConf::Dimension _dimension; 23 | EuclideanSpaceConf(int dimension = IDimension) : _dimension(dimension) {} 24 | typename ParentConf::Dimension getDimension()const { return _dimension; } 25 | typename ParentConf::Dimension getPointSize()const { return getDimension(); } 26 | }; 27 | 28 | template 29 | class DiffManifold< EuclideanSpaceConf, TConfigurationDerived> : public DiffManifold::ParentConf, TConfigurationDerived> { 30 | public: 31 | typedef DiffManifold::ParentConf, TConfigurationDerived> parent_t; 32 | typedef TConfigurationDerived configuration_t; 33 | typedef internal::DiffManifoldConfigurationTypeTrait Types; 34 | typedef typename Types::scalar_t scalar_t; 35 | typedef typename Types::point_t point_t; 36 | typedef typename Types::tangent_vector_t tangent_vector_t; 37 | typedef typename Types::dmatrix_t dmatrix_t; 38 | typedef typename Types::dmatrix_transposed_t dmatrix_transposed_t; 39 | typedef typename Types::dmatrix_point2point_t dmatrix_point2point_t; 40 | 41 | DiffManifold(configuration_t configuration) : parent_t(configuration) {} 42 | DiffManifold(int dimension = configuration_t::Dimension::VALUE) : parent_t(configuration_t(dimension)) {} 43 | 44 | inline void getIdentityInto(point_t & pt) const; 45 | inline void randomizePoint(point_t & pt) const; 46 | 47 | inline void multInto(const point_t & a, const point_t & b, point_t & result) const; 48 | inline dmatrix_point2point_t dMultL(const point_t & mult, bool oppositeMult) const; 49 | 50 | inline void invertInto(const point_t & p, point_t & result) const; 51 | inline void logInto(const point_t & from, const point_t & to, tangent_vector_t & result) const; 52 | inline void logAtIdInto(const point_t & to, tangent_vector_t & result) const; 53 | inline void dlogInto(const point_t & point, const tangent_vector_t & vec, dmatrix_transposed_t & result) const; 54 | inline void dlogAtIdInto(const tangent_vector_t & vec, dmatrix_transposed_t & result) const; 55 | 56 | inline void expInto(const point_t & point, const tangent_vector_t & vec, point_t & result) const; 57 | inline void expAtIdInto(const tangent_vector_t & vec, point_t & result) const; 58 | inline void dexpAtIdInto(const tangent_vector_t & vec, dmatrix_t & result) const; 59 | inline void dexpInto(const point_t & from, const tangent_vector_t & vec, dmatrix_t & result) const; 60 | }; 61 | } 62 | 63 | #include "implementation/EuclideanSpaceImpl.hpp" 64 | #endif /* FLATRIEMANNIANGEOMETRY_HPP_ */ 65 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/manifolds/LieGroup.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LieGroup.hpp 3 | * 4 | * Created on: 28.07.2012 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef LIEGROUP_HPP_ 9 | #define LIEGROUP_HPP_ 10 | 11 | #include "DiffManifold.hpp" 12 | 13 | namespace manifolds { 14 | 15 | template 16 | struct LieGroupConf : public DiffManifoldConfiguration { 17 | typedef LieGroupConf Conf; 18 | typedef DiffManifoldConfiguration ParentConf; 19 | typedef DiffManifold Manifold; 20 | }; 21 | 22 | namespace internal{ 23 | template 24 | struct DiffManifoldPointUpdateTraits, TConfigurationDerived> { 25 | typedef manifolds::internal::DiffManifoldConfigurationTypeTrait Types; 26 | typedef typename Types::point_t point_t; 27 | typedef typename Types::tangent_vector_t tangent_vector_t; 28 | typedef typename TConfigurationDerived::Manifold Manifold; 29 | static inline void update(const Manifold & manifold, point_t & point, const tangent_vector_t & vec){ 30 | // manifold.expInto(point, vec, point); TODO improve: cleanup the mixture of left and right canonical connection in the LieGroup BSpline algorithms. This can then become just exp. 31 | point = manifold.mult(manifold.expAtId(vec), point); 32 | manifold.projectIntoManifold(point); 33 | } 34 | 35 | static inline void minimalDifference(const Manifold & manifold, const Eigen::MatrixXd& xHat, const point_t & to, Eigen::VectorXd& outDifference) { 36 | SM_ASSERT_TRUE(std::runtime_error, (xHat.rows() == manifold.getPointSize()) && (xHat.cols() == 1), "xHat has incompatible dimensions"); 37 | outDifference = manifold.logAtId(manifold.mult(to, manifold.invert(xHat))); 38 | }; 39 | 40 | static inline void minimalDifferenceAndJacobian(const Manifold & manifold, const Eigen::MatrixXd& xHat, const point_t & to, Eigen::VectorXd& outDifference, Eigen::MatrixXd& outJacobian) { 41 | SM_ASSERT_TRUE(std::runtime_error, (xHat.rows() == manifold.getPointSize()) && (xHat.cols() == 1), "xHat has incompatible dimensions"); 42 | auto invertedXHat = manifold.invert(xHat); 43 | auto quot = manifold.mult(to, invertedXHat); 44 | outDifference = manifold.logAtId(quot); 45 | outJacobian = manifold.dlogAtId(quot) * manifold.dMultL(quot, true) * manifold.dexpAtId(Manifold::tangent_vector_t::Zero((int)manifold.getDimension())); //TODO optimize : this is quite slow (V matrix not directly used) 46 | }; 47 | }; 48 | } 49 | 50 | //TODO improve names / hierarchy : this is LieGroup is more a LieGroup with an affine connection (because it has geometric exp, log). This is quite unclean. It would be better to have an additional affine connection type attached to it somehow. 51 | 52 | template 53 | class DiffManifold, TConfigurationDerived> : public DiffManifold::ParentConf, TConfigurationDerived> { 54 | public: 55 | typedef DiffManifold::ParentConf, TConfigurationDerived> parent_t; 56 | typedef TConfigurationDerived configuration_t; 57 | typedef internal::DiffManifoldConfigurationTypeTrait Types; 58 | typedef typename Types::scalar_t scalar_t; 59 | typedef typename Types::point_t point_t; 60 | typedef typename Types::tangent_vector_t tangent_vector_t; 61 | typedef typename Types::dmatrix_t dmatrix_t; 62 | typedef typename Types::dmatrix_point2point_t dmatrix_point2point_t; 63 | typedef typename Types::dmatrix_transposed_t dmatrix_transposed_t; 64 | typedef typename configuration_t::Manifold base_t; 65 | 66 | static constexpr inline CanonicalConnection getCanonicalConnection() { return CanonicalConnection::LEFT; } 67 | 68 | DiffManifold(TConfigurationDerived confiuration) : parent_t (confiuration) {} 69 | 70 | void getIdentityInto(point_t & p) const; 71 | point_t getIdentity() const; 72 | point_t getDefaultPoint() const; 73 | 74 | void multInto(const point_t & a, const point_t & b, point_t & result) const; 75 | inline void multIntoCO(const point_t & a, const point_t & b, point_t & result, bool oppositeMult) const { if(oppositeMult) this->getDerived().multInto(b, a, result); else this->getDerived().multInto(a, b, result); } 76 | point_t mult(const point_t & a, const point_t & b, bool oppositeMult = false) const; 77 | inline void canonicalMultInto(const point_t & mult, const point_t & point, point_t & result) const { multIntoCO(mult, point, result, this->getDerived().getCanonicalConnection() == CanonicalConnection::RIGHT); } 78 | inline point_t canonicalMult(const point_t & mult, const point_t & point) const { return this->getDerived().mult(mult, point, this->getDerived().getCanonicalConnection() == CanonicalConnection::RIGHT); } 79 | dmatrix_point2point_t dMultL(const point_t & mult, bool oppositeMult = false) const; 80 | inline dmatrix_point2point_t dCanonicalMult(const point_t & mult) const { return this->getDerived().dMultL(mult, this->getDerived().getCanonicalConnection() == CanonicalConnection::RIGHT); } 81 | 82 | void invertInto(const point_t & p, point_t & result) const; 83 | point_t invert(const point_t & p) const; 84 | 85 | void expAtIdInto(const tangent_vector_t & vec, point_t & result) const; 86 | void expInto(const point_t & point, const tangent_vector_t & vec, point_t & result) const; 87 | void dexpAtIdInto(const tangent_vector_t & vec, dmatrix_t & result) const; 88 | void dexpInto(const point_t & point, const tangent_vector_t & vec, dmatrix_t & result) const; 89 | 90 | point_t expAtId(const tangent_vector_t & vec) const; 91 | point_t exp(const point_t & point, const tangent_vector_t & vec) const; 92 | dmatrix_t dexpAtId(const tangent_vector_t & vec) const; 93 | dmatrix_t dexp(const point_t & point, const tangent_vector_t & vec) const; 94 | 95 | void logAtIdInto(const point_t & to, tangent_vector_t & result) const; 96 | void logInto(const point_t & from, const point_t & to, tangent_vector_t & result) const; 97 | void dlogInto(const point_t & point, const point_t & to, dmatrix_transposed_t & result) const; 98 | void dlogAtIdInto(const tangent_vector_t & vec, dmatrix_transposed_t & result) const; 99 | 100 | tangent_vector_t logAtId(const point_t & to) const; 101 | tangent_vector_t log(const point_t & from, const point_t & to) const; 102 | dmatrix_transposed_t dlogAtId(const point_t & to) const; 103 | dmatrix_transposed_t dlog(const point_t & from, const point_t & to) const; 104 | }; 105 | } 106 | 107 | #include "implementation/LieGroupImpl.hpp" 108 | 109 | #endif /* LIEGROUP_HPP_ */ 110 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/manifolds/UnitQuaternionManifold.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * UnitQuaternionDiffManifold.hpp 3 | * 4 | * Created on: Jul 26, 2012 5 | * Author: hannes 6 | */ 7 | 8 | #ifndef UNITQUATERNIONGEOMETRY_HPP_ 9 | #define UNITQUATERNIONGEOMETRY_HPP_ 10 | 11 | #include "LieGroup.hpp" 12 | #include 13 | 14 | namespace manifolds { 15 | template 16 | struct UnitQuaternionManifoldConf : public LieGroupConf<3, 4, TScalar> { 17 | typedef LieGroupConf<3, 4, TScalar> ParentConf; 18 | typedef DiffManifold ParentManifold; 19 | typedef UnitQuaternionManifoldConf Conf; 20 | typedef DiffManifold Manifold; 21 | 22 | typename ParentConf::Dimension getDimension() const { return ParentConf::Dimension::VALUE; } 23 | typename ParentConf::PointSize getPointSize() const { return ParentConf::PointSize::VALUE; } 24 | }; 25 | 26 | template 27 | class DiffManifold< UnitQuaternionManifoldConf, TConfigurationDerived> : public DiffManifold::ParentConf, TConfigurationDerived> { 28 | public: 29 | typedef DiffManifold::ParentConf, TConfigurationDerived> parent_t; 30 | typedef TConfigurationDerived configuration_t; 31 | typedef internal::DiffManifoldConfigurationTypeTrait Types; 32 | typedef typename Types::scalar_t scalar_t; 33 | typedef typename Types::point_t point_t; 34 | typedef typename Types::tangent_vector_t tangent_vector_t; 35 | typedef typename Types::dmatrix_t dmatrix_t; 36 | typedef typename Types::dmatrix_transposed_t dmatrix_transposed_t; 37 | typedef typename Types::dmatrix_point2point_t dmatrix_point2point_t; 38 | 39 | DiffManifold(configuration_t configuration) : parent_t(configuration) {}; 40 | DiffManifold() : parent_t(configuration_t()) {}; 41 | 42 | static const dmatrix_t & V(); 43 | static Eigen::Matrix3d S(const tangent_vector_t & vec); 44 | static Eigen::Matrix3d LByVec(const tangent_vector_t & vec); 45 | 46 | static void getIdentityInto(point_t & pt); 47 | static bool isInManifold(const point_t & pt); 48 | static void projectIntoManifold(point_t & pt); 49 | static void randomizePoint(point_t & pt); 50 | 51 | static void multInto(const point_t & a, const point_t & b, point_t & result); 52 | inline dmatrix_point2point_t dMultL(const point_t & mult, bool oppositeMult) const; 53 | inline void invertInto(const point_t & p, point_t & result) const; 54 | 55 | static void expAtIdInto(const tangent_vector_t & vec, point_t & result); 56 | static point_t expAtId(const tangent_vector_t & vec); 57 | void dexpAtIdInto(const tangent_vector_t & vec, dmatrix_t & result) const; 58 | 59 | static void logAtIdInto(const point_t & to, tangent_vector_t & result); 60 | void dlogAtIdInto(const point_t & to, dmatrix_transposed_t & result) const; 61 | }; 62 | } 63 | 64 | #include "implementation/UnitQuaternionManifoldImpl.hpp" 65 | #endif /* UNITQUATERNIONGEOMETRY_HPP_ */ 66 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/manifolds/implementation/EuclideanSpaceImpl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * EuclideanSpaceImpl.hpp 3 | * 4 | * Created on: Jul 26, 2012 5 | * Author: hannes 6 | */ 7 | 8 | 9 | #include "bsplines/manifolds/EuclideanSpace.hpp" 10 | 11 | namespace manifolds { 12 | #define _TEMPLATE template 13 | #define _CLASS DiffManifold, TConfigurationDerived> 14 | 15 | _TEMPLATE 16 | inline void _CLASS::getIdentityInto(point_t & pt) const { 17 | pt = point_t::Zero((int)this->getPointSize()); 18 | } 19 | 20 | _TEMPLATE 21 | inline void _CLASS::multInto(const point_t & a, const point_t & b, point_t & result) const { 22 | result = a + b; 23 | } 24 | 25 | _TEMPLATE 26 | inline typename _CLASS::dmatrix_point2point_t _CLASS::dMultL(const point_t & /* mult */, bool /* oppositeMult */ ) const 27 | { 28 | return _CLASS::dmatrix_point2point_t::Identity((int)this->getPointSize(), (int)this->getPointSize()); 29 | } 30 | 31 | _TEMPLATE 32 | inline void _CLASS::invertInto(const point_t & p, point_t & result) const { 33 | result = -p; 34 | } 35 | 36 | _TEMPLATE 37 | inline void _CLASS::logInto(const point_t & from, const point_t & to, tangent_vector_t & result) const { 38 | result = to - from; 39 | } 40 | 41 | _TEMPLATE 42 | void _CLASS::logAtIdInto(const point_t & to, tangent_vector_t & result) const{ 43 | result = to; 44 | } 45 | 46 | _TEMPLATE 47 | void _CLASS::dlogInto(const point_t & /*point*/, const tangent_vector_t & /*vec*/, dmatrix_transposed_t & result) const{ 48 | result = dmatrix_t::Identity(result.rows(), result.cols()); 49 | } 50 | 51 | _TEMPLATE 52 | void _CLASS::dlogAtIdInto(const tangent_vector_t & /* vec */, dmatrix_transposed_t & result) const { 53 | result = dmatrix_t::Identity(result.rows(), result.cols()); 54 | } 55 | 56 | _TEMPLATE 57 | inline void _CLASS::expInto(const point_t & point, const tangent_vector_t & vec, point_t & result) const { 58 | result = point + vec; 59 | } 60 | 61 | _TEMPLATE 62 | inline void _CLASS::expAtIdInto(const tangent_vector_t & vec, point_t & p) const { 63 | p = vec; 64 | } 65 | 66 | _TEMPLATE 67 | inline void _CLASS::dexpAtIdInto(const tangent_vector_t & /* vec */, dmatrix_t & result) const { 68 | result = dmatrix_t::Identity(result.rows(), result.cols()); 69 | } 70 | 71 | _TEMPLATE 72 | inline void _CLASS::dexpInto(const point_t & /* from */, const tangent_vector_t & /* vec */, dmatrix_t & result) const { 73 | result = dmatrix_t::Identity(result.rows(), result.cols()); 74 | } 75 | 76 | _TEMPLATE 77 | inline void _CLASS::randomizePoint(point_t & pt) const { 78 | pt = 2 * point_t::Random((int)this->getDimension()) - point_t::Ones((int)this->getDimension()); 79 | } 80 | 81 | #undef _CLASS 82 | #undef _TEMPLATE 83 | } 84 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/manifolds/implementation/LieGroupImpl.hpp: -------------------------------------------------------------------------------- 1 | 2 | #include "bsplines/manifolds/LieGroup.hpp" 3 | 4 | namespace manifolds { 5 | #define _TEMPLATE template 6 | #define _CLASS DiffManifold< LieGroupConf, TConfigurationDerived> 7 | 8 | _TEMPLATE 9 | inline typename _CLASS::point_t _CLASS::getIdentity() const 10 | { 11 | point_t p((int)this->getPointSize()); 12 | this->getDerived().getIdentityInto(p); 13 | return p; 14 | } 15 | 16 | _TEMPLATE 17 | inline typename _CLASS::point_t _CLASS::getDefaultPoint() const 18 | { 19 | return this->getDerived().getIdentity(); 20 | } 21 | 22 | _TEMPLATE 23 | inline typename _CLASS::point_t _CLASS::mult(const point_t & a, const point_t & b, bool oppositeMult) const 24 | { 25 | point_t p(a.rows()); 26 | this->getDerived().multIntoCO(a, b, p, oppositeMult); 27 | return p; 28 | } 29 | 30 | _TEMPLATE 31 | inline void _CLASS::expInto(const point_t & point, const tangent_vector_t & vec, point_t & result) const 32 | { 33 | this->getDerived().canonicalMultInto(point, this->getDerived().expAtId(vec), result); 34 | } 35 | 36 | _TEMPLATE 37 | inline typename _CLASS::point_t _CLASS::exp(const point_t & point, const tangent_vector_t & vec) const 38 | { 39 | point_t p(point.rows()); 40 | this->getDerived().expInto(point, vec, p); 41 | return p; 42 | } 43 | 44 | _TEMPLATE 45 | inline typename _CLASS::point_t _CLASS::expAtId(const tangent_vector_t & vec) const 46 | { 47 | point_t result((int)this->getPointSize()); 48 | this->getDerived().expAtIdInto(vec, result); 49 | return result; 50 | } 51 | 52 | _TEMPLATE 53 | inline typename _CLASS::dmatrix_t _CLASS::dexpAtId(const tangent_vector_t & vec) const 54 | { 55 | dmatrix_t result((int)this->getPointSize(), (int)this->getDimension()); 56 | this->getDerived().dexpAtIdInto(vec, result); 57 | return result; 58 | } 59 | 60 | _TEMPLATE 61 | inline void _CLASS::dexpInto(const point_t & point, const tangent_vector_t & vec, dmatrix_t & result) const 62 | { 63 | result = this->getDerived().dCanonicalMult(point) * this->getDerived().dexpAtId(vec); 64 | } 65 | 66 | _TEMPLATE 67 | inline typename _CLASS::dmatrix_t _CLASS::dexp(const point_t & point, const tangent_vector_t & vec) const 68 | { 69 | dmatrix_t result((int)this->getPointSize(), (int)this->getDimension()); 70 | this->getDerived().dexpInto(point, vec, result); 71 | return result; 72 | } 73 | 74 | _TEMPLATE 75 | inline typename _CLASS::tangent_vector_t _CLASS::log(const point_t & from, const point_t & to) const { 76 | tangent_vector_t v((int)this->getDimension()); 77 | this->getDerived().logInto(from, to, v); 78 | return v; 79 | } 80 | 81 | _TEMPLATE 82 | inline typename _CLASS::tangent_vector_t _CLASS::logAtId(const point_t & to) const { 83 | tangent_vector_t v((int)this->getDimension()); 84 | this->getDerived().logAtIdInto(to, v); 85 | return v; 86 | } 87 | 88 | _TEMPLATE 89 | inline void _CLASS::logInto(const point_t & from, const point_t & to, tangent_vector_t & result) const { 90 | auto & d = this->getDerived(); 91 | d.logAtIdInto(d.canonicalMult(d.invert(from), to), result); 92 | } 93 | 94 | _TEMPLATE 95 | inline typename _CLASS::dmatrix_transposed_t _CLASS::dlogAtId(const point_t & to) const { 96 | dmatrix_transposed_t result((int)this->getDimension(), (int)this->getPointSize()); 97 | this->getDerived().dlogAtIdInto(to, result); 98 | return result; 99 | } 100 | 101 | _TEMPLATE 102 | void _CLASS::dlogInto(const point_t & from, const point_t & to, dmatrix_transposed_t & result) const { 103 | auto & d = this->getDerived(); 104 | auto fromInv = d.invert(from); 105 | result = d.dlogAtId(d.canonicalMult(fromInv, to)) * d.dCanonicalMult(fromInv); 106 | } 107 | 108 | _TEMPLATE 109 | inline typename _CLASS::dmatrix_transposed_t _CLASS::dlog(const point_t & from, const point_t & to) const { 110 | dmatrix_transposed_t result((int)this->getDimension(), (int)this->getPointSize()); 111 | this->getDerived().dlogInto(from, to, result); 112 | return result; 113 | } 114 | 115 | _TEMPLATE 116 | inline typename _CLASS::point_t _CLASS::invert(const point_t & p) const 117 | { 118 | point_t result((int)this->getPointSize()); 119 | this->getDerived().invertInto(p, result); 120 | return result; 121 | } 122 | 123 | #undef _CLASS 124 | #undef _TEMPLATE 125 | } 126 | -------------------------------------------------------------------------------- /bsplines/include/bsplines/manifolds/implementation/UnitQuaternionManifoldImpl.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * UnitQuaternionBSpline.hpp 3 | * 4 | * Created on: May 10, 2012 5 | * Author: hannes 6 | */ 7 | 8 | 9 | #include 10 | #include 11 | 12 | #include "bsplines/manifolds/UnitQuaternionManifold.hpp" 13 | 14 | 15 | namespace manifolds { 16 | 17 | #define _TEMPLATE template 18 | #define _CLASS DiffManifold, TConfigurationDerived> 19 | 20 | _TEMPLATE 21 | inline void _CLASS::multInto(const point_t & a, const point_t & b, point_t & result) 22 | { 23 | result = ::sm::kinematics::qplus(a, b); 24 | } 25 | 26 | _TEMPLATE 27 | inline typename _CLASS::dmatrix_point2point_t _CLASS::dMultL(const point_t & mult, bool oppositeMult) const 28 | { 29 | return !oppositeMult ? ::sm::kinematics::quatPlus(mult) : ::sm::kinematics::quatOPlus(mult); 30 | } 31 | 32 | _TEMPLATE 33 | inline void _CLASS::invertInto(const point_t & p, point_t & result) const 34 | { 35 | result = ::sm::kinematics::quatInv(p); 36 | } 37 | 38 | _TEMPLATE 39 | inline void _CLASS::logAtIdInto(const point_t & to, tangent_vector_t & result) 40 | { 41 | result = ::sm::kinematics::quat2AxisAngle(to); 42 | } 43 | 44 | _TEMPLATE 45 | inline typename _CLASS::point_t _CLASS::expAtId(const tangent_vector_t & vec) 46 | { 47 | return ::sm::kinematics::axisAngle2quat(vec); 48 | } 49 | 50 | _TEMPLATE 51 | inline void _CLASS::expAtIdInto(const tangent_vector_t & vec, point_t & result) 52 | { 53 | result = expAtId(vec); 54 | } 55 | 56 | _TEMPLATE 57 | inline void _CLASS::getIdentityInto(point_t & result) { 58 | result = ::sm::kinematics::quatIdentity(); 59 | } 60 | 61 | _TEMPLATE 62 | inline const typename _CLASS::dmatrix_t & _CLASS::V(){ 63 | return sm::kinematics::quatV(); 64 | } 65 | 66 | _TEMPLATE 67 | inline Eigen::Matrix3d _CLASS::S(const tangent_vector_t & vec){ 68 | return ::sm::kinematics::expDiffMat(vec); 69 | } 70 | 71 | _TEMPLATE 72 | inline Eigen::Matrix3d _CLASS::LByVec(const tangent_vector_t & vec){ 73 | return ::sm::kinematics::logDiffMat(vec); 74 | } 75 | 76 | _TEMPLATE 77 | void _CLASS::dlogAtIdInto(const point_t & to, dmatrix_transposed_t & result) const { 78 | result = ::sm::kinematics::quatLogJacobian2(to); 79 | } 80 | 81 | _TEMPLATE 82 | inline void _CLASS::dexpAtIdInto(const tangent_vector_t & vec, dmatrix_t & result) const 83 | { 84 | result = ::sm::kinematics::quatExpJacobian(vec); 85 | } 86 | 87 | 88 | _TEMPLATE 89 | bool _CLASS::isInManifold(const point_t & pt) 90 | { 91 | return fabs(pt.norm() - 1) < fabs(1E-9); 92 | } 93 | 94 | _TEMPLATE 95 | void _CLASS::projectIntoManifold(point_t & pt) 96 | { 97 | SM_ASSERT_GT_DBG(std::runtime_error, pt.norm(), 1E-3, "This quaternion cannot be projected into the unit quaternions: " << pt); 98 | pt /= pt.norm(); 99 | } 100 | 101 | _TEMPLATE 102 | void _CLASS::randomizePoint(point_t & pt) 103 | { 104 | double norm; 105 | do{ //TODO improve : realize uniform distribution on S3 106 | pt = 5*(Eigen::Vector4d::Random() * 2 - Eigen::Vector4d::Ones()); 107 | }while((norm = pt.norm()) < 1e-10); 108 | pt /= norm; 109 | } 110 | 111 | #undef _TEMPLATE 112 | #undef _CLASS 113 | } 114 | -------------------------------------------------------------------------------- /bsplines/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b splines is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /bsplines/package.xml: -------------------------------------------------------------------------------- 1 | 2 | bsplines 3 | 4 | bsplines 5 | 6 | Paul Furgale 7 | 0.0.1 8 | BSD-3-Clause 9 | catkin 10 | catkin_simple 11 | 12 | sm_common 13 | sm_timing 14 | sm_eigen 15 | sm_kinematics 16 | sparse_block_matrix 17 | eigen_catkin 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /bsplines/src/DiffManifoldBSpline.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * DiffManifoldBSpline.cpp 3 | * 4 | * Created on: 05.08.2012 5 | * Author: hannes 6 | * 7 | * this file only supplies the eclipse C-Indexer with a canonical context for the included header 8 | */ 9 | 10 | 11 | 12 | 13 | #include 14 | -------------------------------------------------------------------------------- /bsplines/test/NodeDistributedCacheTests.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * NodeDistributedCacheTests.cpp 3 | * 4 | * Created on: Oct 4, 2012 5 | * Author: hannes 6 | */ 7 | #include "gtest/gtest.h" 8 | #include "bsplines/NodeDistributedCache.hpp" 9 | #include 10 | 11 | using namespace nodecache; 12 | 13 | struct TestNode { 14 | NodeDistributedCache::PerNodeCache cache; 15 | public: 16 | NodeDistributedCache::PerNodeCache & accessCache(){ 17 | return cache; 18 | } 19 | }; 20 | 21 | struct TestValue{ 22 | int i; 23 | 24 | TestValue(int i = 0) : i (i){ 25 | } 26 | 27 | TestValue(const TestValue & v) : i (v.i){ 28 | } 29 | 30 | ~TestValue(){ 31 | } 32 | }; 33 | 34 | typedef NodeDistributedCache TestCache; 35 | 36 | TEST(NodeDistributedCacheTestSuite, testInitialization) 37 | { 38 | TestCache cache; 39 | 40 | { 41 | auto testSlot = cache.registerCacheableValue(); 42 | 43 | TestNode testNode; 44 | 45 | int val = 3238; 46 | testSlot->accessValue(testNode) = TestValue(val); 47 | 48 | SM_ASSERT_EQ(std::runtime_error, testSlot->accessValue(testNode).i , val, ""); 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /bsplines/test/NumericIntegratorTests.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | #include "bsplines/NumericIntegrator.hpp" 3 | #include 4 | #include 5 | 6 | using namespace numeric_integrator; 7 | 8 | const double bounds[] = {-10, -2, -0.5, 0, 0.5, 2, 10}; 9 | const int numberOfBounds = sizeof(bounds) / sizeof(double); 10 | 11 | 12 | double fabs(Eigen::Vector3d a){ 13 | return a.norm(); 14 | } 15 | 16 | 17 | template 18 | struct ZeroGetter{ 19 | static TValue getZero(){ 20 | return static_cast(0); 21 | } 22 | }; 23 | 24 | 25 | template <> 26 | struct ZeroGetter{ 27 | inline static Eigen::Vector3d getZero(){ 28 | return Eigen::Vector3d::Zero(); 29 | } 30 | }; 31 | 32 | template 33 | void checkIntegral(TFunctor & f){ 34 | for(int i = 0; i < numberOfBounds; i ++){ 35 | for(int j = 0; j < numberOfBounds; j ++){ 36 | double lBound = bounds[i], uBound = bounds[j]; 37 | TValue val = integrateFunctor(lBound, uBound, f, INumberOfPoints, ZeroGetter::getZero()); 38 | TValue analyticalVal = f.calcIntegral(lBound, uBound); 39 | SM_ASSERT_NEAR(std::runtime_error, val, analyticalVal, fmax(fabs(analyticalVal) * 1e-3, 1e-6), f.getName()); 40 | } 41 | } 42 | } 43 | 44 | template 45 | void checkIntegral(TFunctor & f){ 46 | checkIntegral(f); 47 | checkIntegral(f); 48 | // checkIntegral(f); 49 | // checkIntegral(f); 50 | } 51 | 52 | 53 | namespace test_functions { 54 | struct XSquared { 55 | typedef double ValueT; 56 | inline double operator () (double x) const { 57 | return x * x; 58 | } 59 | 60 | inline double calcIntegral(double a, double b){ 61 | return (b * b * b - a * a * a) / 3.; 62 | } 63 | inline const char * getName() { return "XSquared"; } 64 | } xSquared; 65 | 66 | struct XCubix{ 67 | typedef double ValueT; 68 | inline double operator () (double x) const { 69 | return x * x * x; 70 | } 71 | 72 | inline double calcIntegral(double a, double b){ 73 | return (b * b * b * b - a * a * a * a) / 4.; 74 | } 75 | inline const char * getName() { return "XCubix"; } 76 | } xCubic; 77 | 78 | struct SinX{ 79 | typedef double ValueT; 80 | inline double operator () (double x)const { 81 | return sin(x); 82 | } 83 | 84 | inline double calcIntegral(double a, double b){ 85 | return cos(a) - cos(b); 86 | } 87 | inline const char * getName() { return "Sin"; } 88 | } sinX; 89 | 90 | struct ExpX{ 91 | typedef double ValueT; 92 | inline double operator () (double x) const { 93 | return exp(x); 94 | } 95 | 96 | inline double calcIntegral(double a, double b){ 97 | return exp(b) - exp(a); 98 | } 99 | inline const char * getName() { return "Exp"; } 100 | } expX; 101 | 102 | struct SinhX{ 103 | typedef double ValueT; 104 | inline double operator () (double x) const { 105 | return sinh(x); 106 | } 107 | inline double calcIntegral(double a, double b){ 108 | return cosh(b) - cosh(a); 109 | } 110 | inline const char * getName() { return "Sinh"; } 111 | } sinhX; 112 | 113 | struct XSquaredInR3{ 114 | typedef Eigen::Vector3d ValueT; 115 | inline ValueT operator () (double x) const { 116 | double v = x * x; 117 | return ValueT(v, 2 * v, 3 * v); 118 | } 119 | 120 | inline ValueT calcIntegral(double a, double b){ 121 | double I = (b * b * b - a * a * a) / 3.; 122 | return ValueT(I, 2 * I, 3 * I); 123 | } 124 | inline const char * getName() { return "XSquaredInR3"; } 125 | } xSquaredInR3; 126 | } 127 | 128 | using namespace test_functions; 129 | 130 | TEST(NumericIntegratorTestSuite, compareWithAnalyticalIntegration) 131 | { 132 | checkIntegral(xSquared); 133 | checkIntegral(xCubic); 134 | checkIntegral(sinX); 135 | checkIntegral(expX); 136 | checkIntegral(sinhX); 137 | } 138 | 139 | TEST(NumericIntegratorTestSuite, eigenVectorValueIntegration) 140 | { 141 | checkIntegral(xSquaredInR3); 142 | } 143 | 144 | TEST(NumericIntegratorTestSuite, testFunctionInterface) 145 | { 146 | struct Integrand { 147 | typedef double ValueT; 148 | inline static ValueT integrand(const double & x){ 149 | return x * x; 150 | } 151 | }; 152 | 153 | integrateFunction(0., 1., Integrand::integrand, 100, 0.); 154 | } 155 | -------------------------------------------------------------------------------- /bsplines/test/UnitQuaternionBSplineTests.cpp: -------------------------------------------------------------------------------- 1 | #include "DiffManifoldBSplineTests.hpp" 2 | #include 3 | 4 | namespace bsplines { 5 | 6 | 7 | TEST(UnitQuaternionBSplineTestSuite, testQuaternionBSplineCompilation) 8 | { 9 | UnitQuaternionBSpline::TYPE rbspline; 10 | const UnitQuaternionBSpline::TYPE::point_t p = UnitQuaternionBSpline::TYPE::point_t(1, 0, 0, 0); 11 | rbspline.initConstantUniformSpline(minTime, maxTime, numberOfSegments, p); 12 | sm::eigen::assertEqual(rbspline.getEvaluatorAt<0>(minTime).eval(), p, SM_SOURCE_FILE_POS); 13 | } 14 | 15 | TEST(UnitQuaternionBSplineTestSuite, differentEvalMethodsEvalTheSame) 16 | { 17 | UQTestSpline spline; 18 | UQTestSplineD splineD(splineOrder); 19 | 20 | initMinimalSpline(spline); 21 | initMinimalSpline(splineD); 22 | 23 | UQTestSpline::point_t p; 24 | for(int i = 0, n = knot_arithmetics::getNumControlVerticesRequired(2, splineOrder) ; i < n; i ++){ 25 | spline.getManifold().randomizePoint(p); 26 | spline.addControlVertex(i, p); 27 | splineD.addControlVertex(i, p); 28 | } 29 | 30 | UQTestSpline::full_jacobian_t jac1, jac2; 31 | UQTestSplineD::full_jacobian_t jac3; 32 | 33 | for(int i = 0, n = 10; i< n; i ++){ 34 | double t = (spline.getMaxTime() - spline.getMinTime()) / (n - 1) * i + spline.getMinTime(); 35 | UQTestSpline::Evaluator eval = spline.getEvaluatorAt(t); 36 | UQTestSplineD::Evaluator eval2 = splineD.getEvaluatorAt(t); 37 | 38 | // std::cout << std::endl << "t = "<< t << std::endl; 39 | sm::eigen::assertNear(eval.evalDRecursive(0), eval.evalGeneric(), 1E-9, SM_SOURCE_FILE_POS); 40 | sm::eigen::assertNear(eval.evalDRecursive(1), eval.evalD1Special(), 1E-9, SM_SOURCE_FILE_POS); 41 | 42 | for(unsigned i = 0 ; i < std::min(splineOrder + 1, unsigned(4)); i ++){ 43 | sm::eigen::assertNear(eval.evalDRecursive(i), eval2.evalDRecursive(i), 0, SM_SOURCE_FILE_POS); 44 | } 45 | for(unsigned i = 0 ; i < std::min(splineOrder + 1, unsigned(3)); i ++){ 46 | //compare generic recursive computation of Jacobian with the optimized one.s 47 | eval.evalJacobianDRecursive(i, jac1); 48 | eval.evalJacobian(i, jac2); 49 | eval2.evalJacobian(i, jac3); 50 | sm::eigen::assertNear(jac1, jac2, 0, SM_SOURCE_FILE_POS); 51 | sm::eigen::assertNear(jac2, jac3, 0, SM_SOURCE_FILE_POS); 52 | } 53 | } 54 | } 55 | 56 | TEST(UnitQuaternionBSplineTestSuite, testDExp) 57 | { 58 | DExpTester::Manifold >::testFunc(100, 10); 59 | } 60 | 61 | TEST(UnitQuaternionBSplineTestSuite, evalRiD1) 62 | { 63 | SplineEvalRiDTester::testFunc(10, 10); 64 | } 65 | TEST(UnitQuaternionBSplineTestSuite, evalRiD2) 66 | { 67 | SplineEvalRiDTester::testFunc(10, 10); 68 | } 69 | TEST(UnitQuaternionBSplineTestSuite, evalRiD3) 70 | { 71 | SplineEvalRiDTester::testFunc(10, 10); 72 | } 73 | 74 | TEST(UnitQuaternionBSplineTestSuite, evalD1) 75 | { 76 | SplineEvalDTester::testFunc(10, 10); 77 | } 78 | 79 | TEST(UnitQuaternionBSplineTestSuite, evalD2) 80 | { 81 | SplineEvalDTester::testFunc(10, 10); 82 | } 83 | 84 | TEST(UnitQuaternionBSplineTestSuite, evalD3) 85 | { 86 | SplineEvalDTester::testFunc(10, 10); 87 | } 88 | 89 | // TODO implement generic angular velocity tests 90 | TEST(UnitQuaternionBSplineTestSuite, evalAngularVelocityAndAcceleration) 91 | { 92 | UQTestSpline spline; 93 | Eigen::Vector3d direction(asin(1) / 2, 0, 0); 94 | 95 | const int numPoints = 4 + splineOrder - 1; 96 | 97 | Eigen::MatrixXd interpolationPointsE = Eigen::MatrixXd::Zero(4, numPoints); 98 | Eigen::VectorXd timesE = Eigen::VectorXd::Zero(numPoints); 99 | 100 | for(int i = -splineOrder + 1; i < numPoints; i++){ 101 | UQTestSpline::point_t p = spline.getManifold().expAtId(direction * (i + splineOrder - 1)); 102 | spline.addControlVertex(i, p); 103 | } 104 | 105 | spline.init(); 106 | assert(spline.getMinTime() == 0.0); 107 | 108 | Eigen::VectorXd accel = Eigen::VectorXd::Zero(3); 109 | 110 | const int numTimes = numPoints * 5; 111 | for(int i = 0; i < numTimes; i ++){ 112 | double t = spline.getMinTime() + (spline.getMaxTime() - spline.getMinTime()) * (double) i / (numTimes - 1); 113 | UQTestSpline::Evaluator<2> eval = spline.getEvaluatorAt<2>(t); 114 | 115 | if(splineOrder == 2){ 116 | sm::eigen::assertNear(eval.eval(), spline.getManifold().expAtId(direction * t), 1E-9, SM_SOURCE_FILE_POS); 117 | sm::eigen::assertNear(spline.getEvaluatorAt<0>(t).eval(), spline.getManifold().expAtId(direction * t), 1E-9, SM_SOURCE_FILE_POS); 118 | } 119 | 120 | sm::eigen::assertNear(eval.evalAngularVelocity(), direction, 1E-9, SM_SOURCE_FILE_POS); 121 | sm::eigen::assertNear(eval.evalAngularAcceleration(), accel, 1E-9, SM_SOURCE_FILE_POS); 122 | } 123 | } 124 | 125 | 126 | TEST(UnitQuaternionBSplineTestSuite, SplineEvalRiDJacobianTesterD0) 127 | { 128 | SplineEvalRiDJacobianTester::testFunc(10, 1); 129 | } 130 | TEST(UnitQuaternionBSplineTestSuite, SplineEvalRiDJacobianTesterD1) 131 | { 132 | SplineEvalRiDJacobianTester::testFunc(10, 1); 133 | } 134 | TEST(UnitQuaternionBSplineTestSuite, SplineEvalRiDJacobianTesterD2) 135 | { 136 | SplineEvalRiDJacobianTester::testFunc(10, 1); 137 | } 138 | 139 | // Check the Jacobian calculation. 140 | TEST(UnitQuaternionBSplineTestSuite, testBSplineJacobianD0) 141 | { 142 | BSplineJacobianTester::testFunc(10, 1); 143 | } 144 | TEST(UnitQuaternionBSplineTestSuite, testBSplineJacobianD1) 145 | { 146 | BSplineJacobianTester::testFunc(10, 1); 147 | } 148 | TEST(UnitQuaternionBSplineTestSuite, testBSplineJacobianD2) 149 | { 150 | BSplineJacobianTester::testFunc(10, 1); 151 | } 152 | 153 | TEST(UnitQuaternionBSplineTestSuite, testAngularVelocitiyJacobian) 154 | { 155 | AngularDerivativeJacobianTestser::testFunc(10, 1); 156 | } 157 | TEST(UnitQuaternionBSplineTestSuite, testAngularAccelerationJacobian) 158 | { 159 | AngularDerivativeJacobianTestser::testFunc(10, 1); 160 | } 161 | 162 | TEST(UnitQuaternionBSplineTestSuite, testDiffManifoldBSplineFitting) 163 | { 164 | double tolerance = 0.3; //TODO improve : the unit quaternion fitting is quite bad. this huge tolerance actually checks almost nothing apart from compilation and running without exceptions. 165 | testDiffManifoldBSplineFitting(numberOfSegments, tolerance); 166 | testDiffManifoldBSplineFitting(numberOfSegments * 2, tolerance); 167 | } 168 | 169 | } // namespace bsplines 170 | -------------------------------------------------------------------------------- /bsplines/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /bsplines_python/.gitignore: -------------------------------------------------------------------------------- 1 | python/bsplines/libbsplines.so 2 | *.pyc 3 | /LocalBuildType.cmake 4 | -------------------------------------------------------------------------------- /bsplines_python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(bsplines_python) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple() 6 | 7 | # Set up the python exports. 8 | SET(PY_PROJECT_NAME bsplines_python) 9 | SET(PY_PACKAGE_DIR python/bsplines) 10 | 11 | add_python_export_library(${PY_PROJECT_NAME} ${PY_PACKAGE_DIR} 12 | src/SplinePython.cpp 13 | src/BSplinePython.cpp 14 | src/DiffManifoldBSplinePython.cpp 15 | src/BSplinePosePython.cpp 16 | ) 17 | 18 | 19 | ## Add nosetest based cpp test target. 20 | catkin_add_nosetests(test/DiffManifoldBSpline.py) 21 | 22 | cs_install() 23 | cs_export() 24 | 25 | 26 | -------------------------------------------------------------------------------- /bsplines_python/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | bsplines_python 4 | 0.0.1 5 | bsplines_python 6 | Paul Furgale 7 | Paul Furgale 8 | BSD-3-Clause 9 | catkin 10 | catkin_simple 11 | 12 | bsplines 13 | numpy_eigen 14 | catkin_boost_python_buildtool 15 | eigen_catkin 16 | sparse_block_matrix 17 | sm_common 18 | sm_kinematics 19 | 20 | -------------------------------------------------------------------------------- /bsplines_python/python/bsplines/__init__.py: -------------------------------------------------------------------------------- 1 | # Import the numpy to Eigen type conversion. 2 | import numpy_eigen 3 | # Import the Spline C++ library. 4 | from .libbsplines_python import * 5 | from .plotPoseSpline import * 6 | -------------------------------------------------------------------------------- /bsplines_python/python/bsplines/plotPoseSpline.py: -------------------------------------------------------------------------------- 1 | import numpy 2 | 3 | def plotPoseSpline(ax, poseSpline, dt=0.1, invert=False, linespec='b-'): 4 | # generate a 3d curve. 5 | curve = [] 6 | if invert: 7 | curve = numpy.array([ poseSpline.inverseTransformation(t)[0:3,3] for t in numpy.append(numpy.arange(poseSpline.t_min(),poseSpline.t_max(),dt),poseSpline.t_max())]) 8 | else: 9 | curve = numpy.array([ poseSpline.position(t) for t in numpy.append(numpy.arange(poseSpline.t_min(),poseSpline.t_max(),dt),poseSpline.t_max())]) 10 | ax.plot3D(curve[:,0], curve[:,1], curve[:,2],linespec) 11 | #print "hello" 12 | #print curve 13 | -------------------------------------------------------------------------------- /bsplines_python/setup.py: -------------------------------------------------------------------------------- 1 | 2 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 3 | 4 | from distutils.core import setup 5 | from catkin_pkg.python_setup import generate_distutils_setup 6 | 7 | # fetch values from package.xml 8 | setup_args = generate_distutils_setup( 9 | packages=['bsplines'], 10 | package_dir={'':'python'}) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /bsplines_python/src/BSplinePosePython.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | 6 | boost::python::tuple orientationAndJacobianWrapper(const bsplines::BSplinePose * bsp, double t) 7 | { 8 | Eigen::MatrixXd J; 9 | Eigen::VectorXi I; 10 | Eigen::Matrix3d C = bsp->orientationAndJacobian(t, &J, &I); 11 | 12 | return boost::python::make_tuple(C,J,I); 13 | } 14 | 15 | boost::python::tuple inverseOrientationAndJacobianWrapper(const bsplines::BSplinePose * bsp, double t) 16 | { 17 | Eigen::MatrixXd J; 18 | Eigen::VectorXi I; 19 | Eigen::Matrix3d C = bsp->inverseOrientationAndJacobian(t, &J, &I); 20 | 21 | return boost::python::make_tuple(C,J,I); 22 | } 23 | 24 | 25 | boost::python::tuple transformationAndJacobianWrapper(const bsplines::BSplinePose * bsp, double t) 26 | { 27 | Eigen::MatrixXd J; 28 | Eigen::VectorXi I; 29 | Eigen::Matrix4d T = bsp->transformationAndJacobian(t, &J, &I); 30 | 31 | return boost::python::make_tuple(T,J,I); 32 | } 33 | 34 | boost::python::tuple inverseTransformationAndJacobianWrapper(const bsplines::BSplinePose * bsp, double t) 35 | { 36 | Eigen::MatrixXd J; 37 | Eigen::VectorXi I; 38 | Eigen::Matrix4d T = bsp->inverseTransformationAndJacobian(t, &J, &I); 39 | 40 | return boost::python::make_tuple(T,J,I); 41 | } 42 | 43 | 44 | boost::python::tuple angularVelocityAndJacobianWrapper(const bsplines::BSplinePose * bsp, double t) 45 | { 46 | Eigen::MatrixXd J; 47 | Eigen::VectorXi coefficientIndices; 48 | Eigen::Vector3d omega = bsp->angularVelocityAndJacobian(t, &J, &coefficientIndices); 49 | //std::cout << "J\n" << J << std::endl; 50 | return boost::python::make_tuple(omega,J,coefficientIndices); 51 | } 52 | 53 | boost::python::tuple angularVelocityBodyFrameAndJacobianWrapper(const bsplines::BSplinePose * bsp, double t) 54 | { 55 | Eigen::MatrixXd J; 56 | Eigen::VectorXi coefficientIndices; 57 | Eigen::Vector3d omega = bsp->angularVelocityBodyFrameAndJacobian(t, &J, &coefficientIndices); 58 | //std::cout << "J\n" << J << std::endl; 59 | return boost::python::make_tuple(omega,J,coefficientIndices); 60 | } 61 | 62 | 63 | boost::python::tuple linearAccelerationAndVelocityWrapper(const bsplines::BSplinePose * bsp, double t) 64 | { 65 | Eigen::MatrixXd J; 66 | Eigen::VectorXi coefficientIndices; 67 | Eigen::Vector3d a = bsp->linearAccelerationAndJacobian(t, &J, &coefficientIndices); 68 | return boost::python::make_tuple(a,J,coefficientIndices); 69 | } 70 | 71 | boost::python::tuple angularAccelerationAndJacobianWrapper(const bsplines::BSplinePose * bsp, double t) 72 | { 73 | Eigen::MatrixXd J; 74 | Eigen::VectorXi coefficientIndices; 75 | Eigen::Vector3d omega = bsp->angularAccelerationAndJacobian(t, &J, &coefficientIndices); 76 | return boost::python::make_tuple(omega,J,coefficientIndices); 77 | } 78 | 79 | boost::python::tuple angularAccelerationBodyFrameAndJacobianWrapper(const bsplines::BSplinePose * bsp, double t) 80 | { 81 | Eigen::MatrixXd J; 82 | Eigen::VectorXi coefficientIndices; 83 | Eigen::Vector3d omega = bsp->angularAccelerationBodyFrameAndJacobian(t, &J, &coefficientIndices); 84 | return boost::python::make_tuple(omega,J,coefficientIndices); 85 | } 86 | 87 | 88 | void import_bspline_pose_python() 89 | { 90 | using namespace bsplines; 91 | using namespace boost::python; 92 | using namespace sm::kinematics; 93 | 94 | 95 | // This initialization actually works! boost::python is amazing. 96 | class_ >("BSplinePose", init()) 97 | .def("transformation",&BSplinePose::transformation) 98 | .def("inverseTransformation",&BSplinePose::inverseTransformation) 99 | .def("initPoseSpline", &BSplinePose::initPoseSpline) 100 | .def("initPoseSpline2", &BSplinePose::initPoseSpline2) 101 | .def("initPoseSpline3", &BSplinePose::initPoseSpline3) 102 | .def("initPoseSplineSparse", &BSplinePose::initPoseSplineSparse) 103 | .def("initPoseSplineSparseKnots", &BSplinePose::initSplineSparseKnots) 104 | .def("addPoseSegment", &BSplinePose::addPoseSegment) 105 | .def("addPoseSegment2", &BSplinePose::addPoseSegment2) 106 | .def("curveValueToTransformation", &BSplinePose::curveValueToTransformation) 107 | .def("transformationToCurveValue", &BSplinePose::transformationToCurveValue) 108 | .def("transformationAndJacobian", &transformationAndJacobianWrapper) 109 | .def("orientation", &BSplinePose::orientation) 110 | .def("orientationAndJacobian", &orientationAndJacobianWrapper) 111 | .def("inverseOrientation", &BSplinePose::inverseOrientation) 112 | .def("inverseOrientationAndJacobian", &inverseOrientationAndJacobianWrapper) 113 | .def("inverseTransformationAndJacobian", &inverseTransformationAndJacobianWrapper) 114 | .def("position", &BSplinePose::position) 115 | .def("linearVelocity", &BSplinePose::linearVelocity) 116 | .def("linearAcceleration", &BSplinePose::linearAcceleration) 117 | .def("angularVelocity", &BSplinePose::angularVelocity) 118 | .def("angularVelocityAndJacobian", &angularVelocityAndJacobianWrapper) 119 | .def("angularVelocityBodyFrame", &BSplinePose::angularVelocityBodyFrame) 120 | .def("angularVelocityBodyFrameAndJacobian", &angularVelocityBodyFrameAndJacobianWrapper) 121 | .def("angularAccelerationBodyFrame", &BSplinePose::angularAccelerationBodyFrame) 122 | .def("angularAccelerationBodyFrameAndJacobian", &angularAccelerationBodyFrameAndJacobianWrapper) 123 | .def("rotation", &BSplinePose::rotation); 124 | //.def("", &BSplinePose::, "") 125 | 126 | } 127 | 128 | -------------------------------------------------------------------------------- /bsplines_python/src/DiffManifoldBSplinePython.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace bspline_exporter; 4 | using namespace bsplines; 5 | using namespace boost::python; 6 | 7 | void import_bspline_diff_manifold_python() 8 | { 9 | boost::python::to_python_converter, DynamicOrTemplateInt_to_python_int>(); 10 | 11 | { 12 | typedef typename EuclideanBSpline<>::TYPE::TimePolicy TimePolicy; 13 | typedef DeltaUniformKnotGenerator KnotGenerator; 14 | class_("DeltaUniformKnotGenerator", init()) 15 | .def("extendBeyondTime", &KnotGenerator::extendBeyondTime) 16 | ; 17 | } 18 | 19 | BSplineExporter::TYPE>::exportEuclideanSpline("EuclideanBSpline"); 20 | BSplineExporter::TYPE>::exportUnitQuaternionSpline("UnitQuaternionBSpline"); 21 | } 22 | -------------------------------------------------------------------------------- /bsplines_python/src/RotationalKinematicsPython.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // A wrapper that gets rid of the second parameter in the parametersToRotationMatrix() function. 9 | template 10 | Eigen::Matrix3d parametersToRotationMatrixWrapper(const ROTATION_TYPE * rt, const Eigen::Vector3d & parameters) 11 | { 12 | return rt->parametersToRotationMatrix(parameters); 13 | } 14 | 15 | template 16 | boost::python::tuple angularVelocityAndJacobianWrapper(const ROTATION_TYPE * rt, const Eigen::Vector3d & p, const Eigen::Vector3d pdot) 17 | { 18 | Eigen::Matrix J; 19 | Eigen::Vector3d omega = rt->angularVelocityAndJacobian(p,pdot,&J); 20 | 21 | return boost::python::make_tuple(omega,J); 22 | } 23 | 24 | 25 | 26 | template 27 | class RotationalKinematicsPythonWrapper 28 | { 29 | public: 30 | typedef ROTATION_TYPE rotation_t; 31 | 32 | static void exportToPython(const std::string & className) 33 | { 34 | using namespace boost::python; 35 | 36 | class_ >(className.c_str()) 37 | .def("parametersToRotationMatrix",¶metersToRotationMatrixWrapper) 38 | .def("rotationMatrixToParameters",&rotation_t::rotationMatrixToParameters) 39 | .def("parametersToSMatrix",&rotation_t::parametersToSMatrix) 40 | .def("angularVelocityAndJacobian",&angularVelocityAndJacobianWrapper); 41 | } 42 | }; 43 | 44 | 45 | void import_rotational_kinematics_python() 46 | { 47 | using namespace boost::python; 48 | class_("RotationalKinematics", no_init); 49 | 50 | 51 | RotationalKinematicsPythonWrapper::exportToPython("EulerAnglesZYX"); 52 | RotationalKinematicsPythonWrapper::exportToPython("EulerAnglesYawPitchRoll"); 53 | RotationalKinematicsPythonWrapper::exportToPython("RotationVector"); 54 | RotationalKinematicsPythonWrapper::exportToPython("EulerRodriguez"); 55 | } 56 | -------------------------------------------------------------------------------- /bsplines_python/src/SplinePython.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | 6 | using namespace bsplines; 7 | using namespace boost::python; 8 | 9 | //typedef UniformCubicBSpline UniformCubicBSplineX; 10 | 11 | 12 | void import_bspline_python(); 13 | //void import_rotational_kinematics_python(); 14 | void import_bspline_pose_python(); 15 | void import_bspline_diff_manifold_python(); 16 | 17 | BOOST_PYTHON_MODULE(libbsplines_python) 18 | { 19 | import_bspline_python(); 20 | import_bspline_pose_python(); 21 | import_bspline_diff_manifold_python(); 22 | 23 | } 24 | -------------------------------------------------------------------------------- /bsplines_python/test/DiffManifoldBSplineFittingExperiments.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | from scipy import integrate 4 | from pylab import arange, array, show, plot, axis 5 | import bsplines 6 | 7 | splineOrder = 4; 8 | s = bsplines.EuclideanBSpline(splineOrder, 1) 9 | 10 | maxTime = 9 11 | minTime = 0 12 | 13 | pos = maxTime; 14 | lamb = 0.00001 15 | goal = 1.0 16 | 17 | knotGenerator = s.initConstantUniformSplineWithKnotDelta(minTime, maxTime, 1, array([0])); 18 | 19 | print("Knots:" + str(s.getKnotsVector())); 20 | 21 | def plotSpline(s, showIt = False): 22 | t = arange(s.getMinTime(), s.getMaxTime(), 0.01) 23 | v = array([ s.eval(tt) for tt in t]) 24 | plot(t, v) 25 | if showIt : show() 26 | 27 | def getAcceleration(s): 28 | return integrate.quad(lambda t: s.evalD(t, 2) * (s.evalD(t, 2)), s.getMinTime(), s.getMaxTime())[0] 29 | 30 | plotSpline(s) 31 | print("AccelerationIntegral:\n", getAcceleration(s)) 32 | 33 | def calcFit(): 34 | goalDist = s.eval(pos)[0] - goal 35 | a = getAcceleration(s) 36 | return goalDist*goalDist - lamb * a 37 | 38 | 39 | s.fitSpline(array([pos]), array([[goal]]), lamb, 0, array([])) 40 | 41 | plotSpline(s) 42 | print("Value:\n", s.eval(pos)); 43 | print("AccelerationIntegral:\n", getAcceleration(s)) 44 | print("Vertices:\n", s.getControlVertices()); 45 | print("Fit:", calcFit()) 46 | 47 | 48 | s.extendAndFitSpline(knotGenerator, array([pos - 1, maxTime * 2]), array([[0, goal]]), lamb, 10) 49 | 50 | plotSpline(s) 51 | print("MaxTime:\n", s.getMaxTime()); 52 | print("Value:\n", s.eval(pos)); 53 | print("AccelerationIntegral:\n", getAcceleration(s)) 54 | print("Vertices:\n", s.getControlVertices()); 55 | print("Fit:", calcFit()) 56 | 57 | if False: 58 | c = s.getControlVertices() 59 | c[maxTime - 1 + 3]+=0.8; 60 | c[maxTime - 2 + 3]-=0.2; 61 | #c[maxTime - 5 + 3]+=0.11404781; 62 | s.setControlVertices(c) 63 | plotSpline(s) 64 | print("Value:\n", s.eval(pos)); 65 | print("AccelerationIntegral:\n", getAcceleration(s)) 66 | print("Vertices:\n", s.getControlVertices()); 67 | 68 | 69 | if True: 70 | s2 = bsplines.EuclideanBSpline(4, 1) 71 | n = 1; 72 | ts=[]; 73 | z = max(0, pos - 4); 74 | ts.extend([ z * float(x)/ n for x in range(0, n + 1, 1) ]) 75 | ts.extend([pos for x in range(0, n + 1, 1)]) 76 | z = maxTime - min(maxTime, pos + 4) 77 | ts.extend([ maxTime - z + z* float(x) / n for x in range(0, n + 1, 1) ]) 78 | values = [ 0 if x != pos else goal for x in ts ] 79 | print ts , values; 80 | 81 | s2.initUniformSpline(array(ts), array([values]), maxTime, lamb * n) 82 | 83 | print "Value:\n", s2.eval(pos); 84 | print "AccelerationIntegral:\n", getAcceleration(s) 85 | print "Vertices:\n", s2.getControlVertices(); 86 | 87 | plotSpline(s2) 88 | 89 | axis([0, maxTime * 2, -0.5, 1.5]) 90 | show() 91 | 92 | print "finished" 93 | -------------------------------------------------------------------------------- /bsplines_python/test/SplineTests.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import asrl_splines 3 | import numpy 4 | import scipy.interpolate.fitpack as fp 5 | import sys 6 | import unittest 7 | 8 | 9 | class TestSplinesVsFitpack(unittest.TestCase): 10 | def test_new_uniform_cubic1(self): 11 | # create the knot and control point vectors 12 | knots = numpy.array([-2.0, -1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) 13 | cp = numpy.array([0.0, 10.0, 2.0, 30.0, 4.0, 50.0]) 14 | cpa = numpy.array([cp]) 15 | 16 | aspl = asrl_splines.UniformCubicBSpline(cpa,1.0,knots[3]); 17 | fspl = (knots,cp,3) 18 | 19 | for i in numpy.arange(2.0,4.0,0.1): 20 | print "Eval at %f\n" % (i) 21 | f = fp.spalde(float(i),fspl) 22 | for j in range(0,3): 23 | a = aspl.evalD(i,j) 24 | assert abs(a - f[j]) < 1e-10, "spline (D%d) evaluated at %f (%f != %f) was not right" % (j,float(i),a,f[j]) 25 | 26 | def test_new_uniform_cubic2(self): 27 | # create the knot and control point vectors 28 | cardinalknots = numpy.array([-2.0, -1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) 29 | cp = numpy.array([0.0, 10.0, 2.0, 30.0, 4.0, 50.0]) 30 | cpa = numpy.array([cp]) 31 | 32 | for dt in numpy.arange(0.1,2.0,0.1): 33 | knots = cardinalknots * dt; 34 | 35 | aspl = asrl_splines.UniformCubicBSpline(cpa,dt,knots[3]); 36 | fspl = (knots,cp,3) 37 | 38 | for i in numpy.arange(2.0*dt,4.0*dt,0.1*dt): 39 | print "Eval at %f\n" % (i) 40 | f = fp.spalde(float(i),fspl) 41 | for j in range(0,3): 42 | a = aspl.evalD(i,j) 43 | assert abs(a - f[j]) < 1e-10, "spline (D%d) evaluated at %f (%f != %f) was not right" % (j,float(i),a,f[j]) 44 | 45 | 46 | def test_new_uniform_cubicIntegral(self): 47 | # create the knot and control point vectors 48 | cardinalknots = numpy.array([-2.0, -1.0, 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]) 49 | cp = numpy.array([0.0, 10.0, 2.0, 30.0, 4.0, 50.0]) 50 | cpa = numpy.array([cp]) 51 | 52 | for dt in numpy.arange(0.1,2.0,0.1): 53 | knots = cardinalknots * dt; 54 | 55 | 56 | aspl = asrl_splines.UniformCubicBSpline(cpa,dt,knots[3]); 57 | fspl = (knots,cp,3) 58 | 59 | for a in numpy.arange(2.0*dt,3.9*dt,0.1*dt): 60 | for i in numpy.arange(2.1*dt,4.0*dt,0.1*dt): 61 | print "Eval at %f\n" % (i) 62 | f = fp.splint(a,float(i),fspl) 63 | b = aspl.evalI(a,i) 64 | assert abs(b - f) < 1e-10, "spline integral evaluated on [%f,%f] (%f != %f) was not right" % (a,i,float(b),f) 65 | 66 | 67 | 68 | 69 | 70 | 71 | if __name__ == '__main__': 72 | import rostest 73 | rostest.rosrun('splines', 'test_vs_fitpack', TestSplinesVsFitpack) 74 | 75 | -------------------------------------------------------------------------------- /bsplines_python/test/UnitTests.py: -------------------------------------------------------------------------------- 1 | from SplineTests import * 2 | from BSplineTests import * 3 | from BSplinePoseTests import * 4 | 5 | if __name__ == '__main__': 6 | import rostest 7 | rostest.rosrun('splines', 'test_vs_fitpack', TestSplinesVsFitpack) 8 | rostest.rosrun('splines', 'bspline_pose', TestBSplinePose) 9 | #rostest.rosrun('splines', 'bspline', TestBSplines) 10 | 11 | 12 | -------------------------------------------------------------------------------- /ci/prepare-build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | if [[ $(uname) == "Linux" ]]; then 3 | sudo apt-get install -y python-scipy 4 | else 5 | echo "Platform $(uname) is not supported!" 6 | fi 7 | -------------------------------------------------------------------------------- /dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: catkin_simple 3 | uri: https://github.com/catkin/catkin_simple.git 4 | - git: 5 | local-name: schweizer_messer 6 | uri: https://github.com/ethz-asl/schweizer_messer.git 7 | - git: 8 | local-name: eigen_catkin 9 | uri: https://github.com/ethz-asl/eigen_catkin.git 10 | - git: 11 | local-name: aslam_optimizer 12 | uri: https://github.com/ethz-asl/aslam_optimizer.git 13 | - git: 14 | local-name: catkin_boost_python_buildtool 15 | uri: https://github.com/ethz-asl/catkin_boost_python_buildtool.git 16 | --------------------------------------------------------------------------------