utiasasrl / steam Goto Github PK

View Code? Open in Web Editor NEW

89.0 26.0 14.0 2.67 MB

The Simultaneous Trajectory Estimation and Mapping (STEAM) Engine.

License: BSD 3-Clause "New" or "Revised" License

CMake 0.94% C++ 99.04% Dockerfile 0.03%

cpp robotics state-estimation

steam's Introduction

steam

NOTE: this branch is kept identical to the master branch, except that USE_AMENT is set to ON.

STEAM (Simultaneous Trajectory Estimation and Mapping) Engine is an optimization library aimed at solving batch nonlinear optimization problems involving both SO(3)/SE(3) and continuous-time components. This is accomplished by using an iterative Gauss-Newton-style estimator in combination with techniques developed and used by ASRL. With respect to SO(3) and SE(3) components, we make use of the constraint sensitive perturbation schemes discussed in Barfoot and Furgale [1]. STEAM Engine is by no means intended to be the fastest car on the track; the intent is simply to be fast enough for the types of problems we wish to solve, while being both readable and easy to use by people with a basic background in robotic state estimation.

[1] Barfoot, T. D. and Furgale, P. T., “Associating Uncertainty with Three-Dimensional Poses for use in Estimation Problems,” IEEE Transactions on Robotics, 2014.

Installation

Dependencies

Compiler with C++17 support and OpenMP
CMake (>=3.16)
Eigen (>=3.3.7)
lgmath (>=1.1.0)
(Optional) ROS2 Foxy or later (colcon+ament_cmake)

Install c++ compiler, cmake and OpenMP

sudo apt -q -y install build-essential cmake libomp-dev

Install Eigen (>=3.3.7)

# using APT
sudo apt -q -y install libeigen3-dev

# OR from source
WORKSPACE=~/workspace  # choose your own workspace directory
mkdir -p ${WORKSPACE}/eigen && cd $_
git clone https://gitlab.com/libeigen/eigen.git . && git checkout 3.3.7
mkdir build && cd $_
cmake .. && make install # default install location is /usr/local/

Note: if installed from source to a custom location then make sure cmake can find it.

Install lgmath

Follow the instructions here.

Build and install steam using `cmake`

WORKSPACE=~/workspace  # choose your own workspace directory
# clone
mkdir -p ${WORKSPACE}/steam && cd $_
git clone https://github.com/utiasASRL/steam.git .
# build and install
mkdir -p build && cd $_
cmake ..
cmake --build .
cmake --install . # (optional) install, default location is /usr/local/

Preprocessor macros

STEAM_DEFAULT_NUM_OPENMP_THREADS=<num. threads>: Default to 4. Define number of threads to be used by OpenMP in STEAM.
STEAM_USE_OBJECT_POOL: Default to undefined. If defined then STEAM will use an object pool to improve performance but is no longer thread safe.

Note: steamConfig.cmake will be generated in both build/ and <install prefix>/lib/cmake/steam/ to be included in other projects.

Build examples

samples/CMakeLists.txt shows an example of how to add steam to your projects.

To build and run these samples:

cd ${WORKSPACE}/steam  ## $WORKSPACE defined above
mkdir -p build_samples && cd $_
cmake ../samples
cmake --build .  # Executables will be generated in build_samples

Build and install steam using `ROS2(colcon+ament_cmake)`

WORKSPACE=~/workspace  # choose your own workspace directory

mkdir -p ${WORKSPACE}/steam && cd $_
git clone https://github.com/utiasASRL/steam.git .

source <your ROS2 worspace that includes steam>
colcon build --symlink-install --cmake-args "-DUSE_AMENT=ON"

Same preprocessor macro mentioned above also apply.

License

steam's People

Contributors

Stargazers

Watchers

Forkers

andre-nguyen neophack liuxinren456852 laplacekorea chengwei920412 zliu45 jiaweimeng ignss hflemmen kumarwzx philip-huang norlab-ulaval williamwenda namdinhrobotics

steam's Issues

How to get trajectory prior cost terms

Hi, we are trying to optimize a trajectory using the following piece of code:

    steam::traj::const_vel::Interface traj;
    steam::OptimizationProblem problem;
    for(int i = 0; i < poses.size(); ++i)
    {
        Eigen::Matrix4d T = poses[i].second.cast<double>();
        std::shared_ptr<steam::se3::SE3StateVar> T_vi = steam::se3::SE3StateVar::MakeShared(lgmath::se3::Transformation(T));
        std::shared_ptr<steam::vspace::VSpaceStateVar<6>> w_iv_inv = steam::vspace::VSpaceStateVar<6>::MakeShared(Eigen::Matrix<double, 6, 1>::Zero());
        if(i == 0)
        {
            w_iv_inv->locked() = true;
        }

        traj.add(steam::traj::Time(poses[i].first), T_vi, w_iv_inv);
        problem.addStateVariable(T_vi);
        problem.addStateVariable(w_iv_inv);

        std::shared_ptr<steam::p2p::P2PErrorEvaluator> errorFunc = steam::p2p::P2PErrorEvaluator::MakeShared(T_vi, Eigen::Vector3d::Zero(), poses[i].second.topRightCorner<3, 1>().cast<double>());
        std::shared_ptr<steam::StaticNoiseModel<3>> noiseModel = steam::StaticNoiseModel<3>::MakeShared(Eigen::Matrix3d::Identity(), steam::NoiseType::INFORMATION);
        std::shared_ptr<steam::L2LossFunc> lossFunc = steam::L2LossFunc::MakeShared();
        std::shared_ptr<steam::WeightedLeastSqCostTerm<3>> costTerm = steam::WeightedLeastSqCostTerm<3>::MakeShared(errorFunc, noiseModel, lossFunc);
        problem.addCostTerm(costTerm);
    }
    steam::GaussNewtonSolver::Params params;
    params.verbose = true;
    steam::GaussNewtonSolver solver(problem, params);
    solver.optimize();

where poses is a vector containing the robot poses and their associated timestamps. It is of type std::vector<std::pair<float, Eigen::Matrix4f>>. However, with this code, we end up with the following error:

terminate called after throwing an instance of 'steam::decomp_failure'
  what():  During steam solve, Eigen LLT decomposition failed. It is possible that the matrix was ill-conditioned, in which case adding a prior may help. On the other hand, it is also possible that the problem you've constructed is not positive semi-definite.

The error message seems to suggest to add a prior to the trajectory, but we are a bit lost as to how to do it. In pysteam, there is a handy function named get_prior_cost_terms() defined in the Interface class which allows to get more cost terms to add to the optimization problem, but we were wondering how to do the same in steam.

Thanks in advance!

PDF documentation

Here some possible enhancement related to Manual.tex:

No documentation for the Continuous-Time Trajectories
I'm missing some code example through the module descriptions instead of only at the end. This could help to link the math with the C++ API.
"we are able to expand drop small terms to find that" -> missing something

There is no empty constructor for VectorSpaceStateVar

steam::se3::VectorSpaceStateVar() does not exist causing problem when integrated in other data structure.

Hard coded paths in samples code

For example, see this line:
https://github.com/utiasASRL/steam/blob/develop/samples/SimpleBAandTrajPrior.cpp#L57

More precisions required for runtime_error

There is two occurrences of this error in the code:
throw std::runtime_error("Requested trajectory evaluator at an invalid time.");

It would be easier to debug if the faulty time would be reported and the limit that it breaks.

Failed to build a image using Dockerfile

Nice work!
I failed to build a image in my environment using dockerfile, because of my poor net. Could you please push and publish your image on dockerhub.

Implementing point-to-point costTerm

This code
https://github.com/utiasASRL/ContinuousTimeRegistration/blob/master/src/SteamedICP.cpp#L230

triggers

Begin Optimization
------------------
Number of States: 1
Number of Cost Terms: 9385
Initial Cost: 366.032
terminate called after throwing an instance of 'steam::decomp_failure'
  what():  During steam solve, Eigen LLT decomposition failed. It is possible that the matrix was ill-conditioned, in which case adding a prior may help. On the other hand, it is also possible that the problem you've constructed is not positive semi-definite.
Aborted (core dumped)

@sanderson77 any thoughts?

catkin_optional repository is private

The catkin_optional dependency is still a private repository.

Trajectory sample code needs update

The example code SimpleBAandTrajPrior.cpp (and maybe other) should not define its own struct for a knot (see line 18). It should directly use the class SteamTrajVar provided in Steam to avoid confusion.

Jenkins support

See utiasASRL/lgmath#21 for an example.

Not thread safe

Kai discovered STEAM is not thread safe and therefore running two copies of STEAM simultaneously causes collisions. Kai has ideas how to fix this.

Unused definitions

There seems to be a bunch of unused definitions triggering build warnings. Comment them out or delete them if they're not needed.

steam/include/steam/evaluator/blockauto/transform/TransformEvalOperations.hpp:22:32: warning: ‘steam::BlockAutomaticEvaluator<lgmath::se3::Transformation, 6, 6>::Ptr steam::se3::compose(const ConstPtr&, const ConstPtr&)’ defined but not used [-Wunused-function]
 static TransformEvaluator::Ptr compose(const TransformEvaluator::ConstPtr& transform_cb,

steam/include/steam/evaluator/blockauto/transform/TransformEvalOperations.hpp:30:32: warning: ‘steam::BlockAutomaticEvaluator<lgmath::se3::Transformation, 6, 6>::Ptr steam::se3::composeInverse(const ConstPtr&, const ConstPtr&)’ defined but not used [-Wunused-function]
 static TransformEvaluator::Ptr composeInverse(const TransformEvaluator::ConstPtr& transform_bx,

steam/include/steam/evaluator/blockauto/transform/TransformEvalOperations.hpp:38:38: warning: ‘steam::se3::ComposeLandmarkEvaluator::Ptr steam::se3::compose(const ConstPtr&, const Ptr&)’ defined but not used [-Wunused-function]
 static ComposeLandmarkEvaluator::Ptr compose(const TransformEvaluator::ConstPtr& transform_ba,

steam/include/steam/evaluator/blockauto/transform/TransformEvalOperations.hpp:46:32: warning: ‘steam::BlockAutomaticEvaluator<lgmath::se3::Transformation, 6, 6>::Ptr steam::se3::inverse(const ConstPtr&)’ defined but not used [-Wunused-function]
 static TransformEvaluator::Ptr inverse(const TransformEvaluator::ConstPtr& transform) {

steam/include/steam/evaluator/blockauto/transform/TransformEvalOperations.hpp:53:29: warning: ‘steam::se3::LogMapEvaluator::Ptr steam::se3::tran2vec(const ConstPtr&)’ defined but not used [-Wunused-function]
 static LogMapEvaluator::Ptr tran2vec(const TransformEvaluator::ConstPtr& transform) {

Covariance interpolation/extrapolation not implemented

Talking to Sean and Mike[0], it seems that the STEAM code currently does not allow for interpolation/extrapolation of the covariance, only the mean. We need to upgrade this to allow for extrapolation to account for localization latency in future.

Add target for Doxygen in CMakeLists.txt

The code seems mostly documented, but there is no Doxygen taget to build it in the CMakeLists.txt.