utiasasrl/steam

The Simultaneous Trajectory Estimation and Mapping (STEAM) Engine.

steam's Issues

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?

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.

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.

Hard coded paths in samples code

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

catkin_optional repository is private

The catkin_optional dependency is still a private repository.

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.

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) {

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

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.

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!

There is no empty constructor for VectorSpaceStateVar

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

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.");

• https://github.com/utiasASRL/steam/blob/develop/src/trajectory/SteamTrajInterface.cpp#L97
• https://github.com/utiasASRL/steam/blob/develop/src/trajectory/SteamTrajInterface.cpp#L119

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

Jenkins support

See utiasASRL/lgmath#21 for an example.

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.