LieVector in GTSAM 4 Issues

[Christian Forster]

I have some issues moving my project to GTSAM 4.

I understand that LieVector will be deprecated. Therefore, when I create for instance a PriorFactor on the velocity of a frame I write:

new_factors_.push_back(
        boost::make_shared<gtsam::PriorFactor< gtsam::Vector3 > >(
              gtsam::Symbol('v', frame->id()),
              gtsam::Vector3(velocity),
              velocity_noise));

However, this gives me the compiler errors:

error: 'const class Eigen::Matrix<double, 3, 1>' has no member named 'print'
/home/cforster/workspace/gtsam/gtsam/slam/PriorFactor.h: In member function 'bool gtsam::PriorFactor<VALUE>::equals(const gtsam::NonlinearFactor&, double) const [with VALUE = Eigen::Matrix<double, 3, 1>]':
...
error: 'const class Eigen::Matrix<double, 3, 1>' has no member named 'equals'
/home/cforster/workspace/gtsam/gtsam/slam/PriorFactor.h: In member function 'gtsam::Vector gtsam::PriorFactor<VALUE>::evaluateError
...
error: 'const T' has no member named 'dim'
...
error: 'const class Eigen::Matrix<double, 3, 1>' has no member named 'localCoordinates'

Therefore, I thought that I might need ChartValues and tried this:

new_factors_.push_back(
        boost::make_shared<gtsam::PriorFactor< gtsam::ChartValue<gtsam::Vector3> > >(
              gtsam::Symbol('v', frame->id()),
              gtsam::ChartValue<gtsam::Vector3>(velocity),
              velocity_noise));

Now, it compiles. But when I run it, I get the following error:

terminate called after throwing an instance of 'tbb::captured_exception'
  what():  Attempting to retrieve value with key "v0", type stored in Values is N5gtsam10ChartValueIN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEENS_12DefaultChartIS3_EEEE but requested type was N5gtsam10ChartValueIN5Eigen6MatrixIdLi3ELi1ELi0ELi3ELi1EEENS_12DefaultChartIS3_EEEE
Aborted (core dumped)

Note that the two types are exactly the same.

The error occurs, when I try to read the velocity:

velocity = 
    estimate_.at< gtsam::ChartValue<gtsam::Vector3> >(
        gtsam::Symbol('v', frame->id())).value();

Does someone have an idea? Thanks in advance for your help!

[Frank Dellaert]

No, Vector3 should work, but PriorFactor has not been fixed to deal with it yet. The printing and equals should be implemented with type::traits. They are implemented in GenericValue.h for Vectors and matrices (this might need to be cleaned up). So, the fix is to use those traits in PriorFactor.h (and BetweenFactor.h). Should be easy. @cforster Do you want to try it?

[Frank Dellaert]

If, so, re-assign to yourself and create a branch/pull request. I'm deep in another branch but even if you have a change that does not compile, feel free to PR that and I (and others) can look at what might be the right way.

[Paul Furgale]

I can fix it. We have someone who ran into this here as well. Hi @cforster!

[Paul Furgale]

@cforster, after the fix, your initial syntax should be correct:

new_factors_.push_back(
        boost::make_shared<gtsam::PriorFactor< gtsam::Vector3 > >(
              gtsam::Symbol('v', frame->id()),
              gtsam::Vector3(velocity),
              velocity_noise));

[Christian Forster]

Thanks for your help!

Hi @ptf, let me know when I can help! I can also try to do it.

[Christian Forster]

has been resolved