Robust smart factors

[Thomas Mörwald]

Hi,

I'm implementing a Visual-Inertial Odometry algorithm using GTSAM and I'm currently playing around with the different factors for visual constraints.

So far I've tested the GenericProjectionFactor and the SmartProjectionPoseFactor. The best results regarding accuracy are obtained with the GenericProjectionFactor in combination with Cauchy or Huber for the noise model.

Now, I would like to take advantage of the smart factors with respect to speed, without sacrificing accuracy. Unfortunately they do not allow a robust noise model. In the paper [1] they state that outlier handling is done within the smart factors.

1. Is this enabled by default?
2. Do smart factors achieve similar accuracy as the GenericProjectionFactor with Huber/Cauchy noise model?
3. Are there any vision factors implementing the spherical parametrization of landmarks, ie. bearing-vector + inverse depth?

[1] L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014

[Frank Dellaert]

smart factors take a sharedNoiseModel, can that *not* be a robust error model? See NoiseModel.h

[Thomas Mörwald]

Thanks for the quick response.

No, it throws when trying to use anything different than Isotropic. See SmartFactorBase.h:

  SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,

                  boost::optional<Pose3> body_P_sensor = boost::none,

                  size_t expectedNumberCameras = 10)

      : body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) {

    if (!sharedNoiseModel)

      throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");

    SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<

        noiseModel::Isotropic>(sharedNoiseModel);

    if (!sharedIsotropic)

      throw std::runtime_error("SmartFactorBase: needs isotropic");

    noiseModel_ = sharedIsotropic;

  }

[Frank Dellaert]

Interesting, I forgot this, and the reason why. If you have time to dig deeper into the reason, I can comment on whether it can be fixed or not.

About inverse depth: I believe we have something in unstable, do a global search ?

[Thomas Mörwald]

Thanks, I'll give feedback once I've found out more about robust smart factors.

[J Zhang]

Hi Thomas, 

I am in the same boat here. I am trying to use a robust kernel for smart factors to deal with outliers and got the same error.

Did you find out more about why the robust kernel is not supported? I like the speed of using smart factors which allows me to smooth longer trajectories. 

Thanks

JZ 

[Bart Gallet]

Howdy folks,

my apologizes for  pulling "an old cow out of the ditch" here.

I am running into the same issue - using the development branch that I checked out in Dec 2022.

I am using the OpenCV KLT feature tracker. It's not bad, but in some situations it messes up causing a IndeterminantLinearSystemException.

I was hoping that I could deal with these outliers by using a Huber (or similar) kernel, while keeping a standard isotropic noise setting for the nominal "well behaved" case - but I can't because of the reasons mentioned above.

Has anybody found a known workaround for this by any chance?

Cheers,

Bart

[Dellaert, Frank]

I don’t see an “above”.

A SmartProjectionFactor takes a sharedNoiseModel right? What happens when you give it a Huber model? I seem to remember something but if you point me to the code it will be faster.

F

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/c37d654d-75b4-4f67-87b9-63f89fceca80n%40googlegroups.com.

[Bart Gallet]

Frank,

I am setting up smart factors with a Huber loss as follows:

```
gtsam::noiseModel::Isotropic::shared_ptr measurementNoise = gtsam::noiseModel::Isotropic::Sigma(2, config_->sigma_camera);
auto huber_loss = ::gtsam::noiseModel::Robust::Create(::gtsam::noiseModel::mEstimator::Huber::Create(1.5*config_->sigma_camera), measurementNoise);

gtsam::Cal3_S2::shared_ptr K(new gtsam::Cal3_S2(config_->camera_fx, config_->camera_fy, config_->camera_skew, config_->camera_x0, config_->camera_y0));
gtsam::Pose3 sensor_P_body = gtsam::Pose3(gtsam::Rot3(config_->R_C0toI), gtsam::Point3(config_->p_IinC0));

SmartFactor::shared_ptr smartfactor(new SmartFactor(huber_loss, K, sensor_P_body.inverse()));

smartfactor->add(gtsam::Point2(feature_uv.at(i).x, feature_uv.at(i).y), X(node_idx_));
graph_.push_back(smartfactor);
```

I am getting the following runtime error:

terminate called after throwing an instance of 'std::runtime_error'
  what():  SmartFactorBase: needs isotropic

As Thomas Mörwald pointed out a few years ago, it throws an exception defined in SmartFactorBase.h

```

  SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,

                  boost::optional<Pose3> body_P_sensor = boost::none,

                  size_t expectedNumberCameras = 10)

      : body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) {

    if (!sharedNoiseModel)

      throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");

    SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<

        noiseModel::Isotropic>(sharedNoiseModel);

    if (!sharedIsotropic)

      throw std::runtime_error("SmartFactorBase: needs isotropic");

    noiseModel_ = sharedIsotropic;

  }

```

Cheers

Bart

[Dellaert, Frank]

Thanks!

 

I remember this now. At the time we wrote smart factors with that assumption. Unfortunately removing that assumption and validating that things keep working is probably a project, not a quick fix. If you are motivated to do it, you could read up on the source code and we can meet to brainstorm about solutions. I will not be able to devote a lot of time to it, though, it’ be up to you (or anyone on this thread who’s motivated and has the time).

 

BTW I was thinking, separately, about the ability to provide a “batch dimension” to any type of factor, which would replace smartfactors/extend it to other types. That is a larger project, and again, looking for collaborators here.

 

Before you commit, though, make sure you can devote the time and have the stamina to finish it 😊

 

Frank

[Aryaman Patel]

What would be the right way to fix this assumption? 

[Dellaert, Frank]

Before answering this, I generated some docs to refresh my memory. They are here. Let me know if you find any issues.

 

Regarding the noise model, the current implementation of SmartFactorBase (and therefore its derivatives like SmartProjectionFactor and SmartProjectionPoseFactor) requires a single, shared, isotropic noise model (like noiseModel::Isotropic::Sigma or noiseModel::Unit::Create) for all measurements associated with that factor instance.

 

I commented in 2015:

/**

   * As of Feb 22, 2015, the noise model is the same for all measurements and

   * is isotropic. This allows for moving most calculations of Schur complement

   * etc. to be easily moved to CameraSet, and also agrees pragmatically

   * with what is normally done.

   */

 

This restriction simplifies the implementation and math, particularly the whitening process applied during the Schur complement calculation. I’m honestly not sure how to fix this, but if you want to do a deeper dive based on the docs and my comments above, go for it.

 

Frank

 

 

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/f2f9a18d-b1f5-4c26-8b91-e0d8e3d4e0aen%40googlegroups.com.

[Bart Gallet]

While the following may not be the most elegant or general solution, it made the difference between a VIO system that worked in simulation and one that performs robustly in the real world.

I created local versions of the following GTSAM headers:
- `smart_factor_base.hpp`
- `smart_projection_factor.hpp`
- `smart_projection_pose_factor.hpp`

In these files, I hardcoded a [Huber loss](https://en.wikipedia.org/wiki/Huber_loss) function into the reprojection error computation. Specifically, I added a method `totalReprojectionErrorWithHuberLoss()` to `smart_projection_factor.hpp`, which computes a weighted reprojection error using the Huber loss. The derived classes now use this method in place of `totalReprojectionError()`.

I'm sharing the implementation below in case it's helpful to others or worth considering for generalization. If there’s interest in integrating this capability into GTSAM more formally, I’d be happy to help—especially with some support and guidance from the core team to ensure it aligns well with the broader design.

```cpp
double totalReprojectionErrorWithHuberLoss(const Cameras& cameras,
                                           boost::optional<Point3> externalPoint = boost::none) const
{
    if (huber_threshold_ <= 0)
        return totalReprojectionError(cameras, externalPoint);

    if (externalPoint)
        result_ = TriangulationResult(*externalPoint);
    else
        result_ = triangulateSafe(cameras);

    if (result_) {
        Vector reprojectionErrors = this->unwhitenedError(cameras, *result_);
        double totalError = 0.0;

        for (int i = 0; i < reprojectionErrors.size(); ++i) {
            double residual = reprojectionErrors(i);
            double huber_weight = (std::abs(residual) <= huber_threshold_)
                                  ? 1.0
                                  : huber_threshold_ / std::abs(residual);
            totalError += 0.5 * huber_weight * residual * residual;
        }

        return totalError;

    } else if (params_.degeneracyMode == HANDLE_INFINITY) {
        Unit3 backprojected = cameras.front().backprojectPointAtInfinity(this->measured_.at(0));
        Vector reprojectionErrors = this->unwhitenedError(cameras, backprojected);

        double totalError = 0.0;
        for (int i = 0; i < reprojectionErrors.size(); ++i) {
            double residual = reprojectionErrors(i);
            double huber_weight = (std::abs(residual) <= huber_threshold_)
                                  ? 1.0
                                  : huber_threshold_ / std::abs(residual);
            totalError += 0.5 * huber_weight * residual * residual;
        }

        return totalError;

    } else {
        return 0.0;
    }

}

```

Thanks again to the GTSAM community for all the great work.

Best regards,
Bart

You received this message because you are subscribed to a topic in the Google Groups "gtsam users" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/gtsam-users/qHXl9RLRxRs/unsubscribe.
To unsubscribe from this group and all its topics, send an email to gtsam-users...@googlegroups.com.
To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/SJ0PR07MB9074142F563453FA63AE48CECF8BA%40SJ0PR07MB9074.namprd07.prod.outlook.com.

[Dellaert, Frank]

Hi Bart

 

We actually have Huber in GTSAM, as well as many other models: https://github.com/borglab/gtsam/blob/develop/gtsam/linear/LossFunctions.h

They are designed to be used with noiseModel::Robust.

 

If you only changed the totalReprojectionError methods, you would not be applying the correct Jacobians for the optimization. But, if it works for you, that’s empirical evidence that approximate gradients are fine in practice.