Rekeying a variable within a factor throws ValuesKeyDoesNotExist error for Key that is not in graph anymore

[bobhendrikx]

When I construct a factor graph, and then change a factor such that a certain key no longer exists in the graph, the solver still tries to access that key from the initial values.
Both removing that value from the keys and leaving it in there throws an error.
Even though the printed graph shows indeed that the variable no longer is present.

I suspect that the copy of the BearingRangeFactor using rekey() still holds the old keys somewhere internally.

Below the code to reproduce the error (output is from older gtsam version but same results with recent 4.0.1 gtsam version)

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

int main(){
    gtsam::NonlinearFactorGraph factorgraph;
    gtsam::Values values;

    // first robot pose at 1 with prior factor
    gtsam::Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
    auto DummyNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.1, 0.1, 0.1));
    factorgraph.emplace_shared<gtsam::PriorFactor<gtsam::Pose2> >(1, priorMean, DummyNoise);
    values.insert(1,gtsam::Pose2(0.0,0.0,0.0));

    // Add Range bearing at 2
    auto DummyNoise2 = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector2(0.1, 0.1));
    factorgraph.emplace_shared<gtsam::BearingRangeFactor<gtsam::Pose2,gtsam::Point2>>(1,2,0.5,1, DummyNoise2);
    values.insert(2,gtsam::Point2(0.0,0.0));

    // Add Range bearing at 3
    factorgraph.emplace_shared<gtsam::BearingRangeFactor<gtsam::Pose2,gtsam::Point2>>(1,3,0.5,1, DummyNoise2);
    values.insert(3,gtsam::Point2(0.0,0.0));
    values.print();

    auto result = gtsam::LevenbergMarquardtOptimizer(factorgraph, values).optimize();

    std::cout << "ORIGINAL FACTOR GRAPH" << std::endl;
    factorgraph.print();

    std::map<gtsam::Key, gtsam::Key> keystochange;
    keystochange.emplace(std::pair<int,int>(2,3));
    factorgraph.replace(1,  factorgraph.at(1)->rekey(keystochange));

    values.erase(2);
    std::cout << "VALUES AFTER ERASING VARIABLE 2" << std::endl;
    values.print();

    std::cout << "FACTOR GRAPH AFTER REPLACING POINT2 VARIABLE 2 WITH 3 IN FACTOR 1" << std::endl;
    factorgraph.print();

    auto result_after_erase = gtsam::LevenbergMarquardtOptimizer(factorgraph, values).optimize(); // Throws ValuesKeyDoesNotExist
    return 0;
}

Output:

Values with 3 values:
Value 1: (N5gtsam5Pose2E) (0, 0, 0)

Value 2: (N5gtsam6Point2E) (0, 0)

Value 3: (N5gtsam6Point2E) (0, 0)

ORIGINAL FACTOR GRAPH
NonlinearFactorGraph: size: 3

Factor 0: PriorFactor on 1
prior mean: (0, 0, 0)
isotropic dim=3 sigma=0.1

Factor 1: BearingRangeFactor
Factor 1: keys = { 1 2 }
isotropic dim=2 sigma=0.1
ExpressionFactor with measurement: bearing : 0.5
range 1.000000

Factor 2: BearingRangeFactor
Factor 2: keys = { 1 3 }
isotropic dim=2 sigma=0.1
ExpressionFactor with measurement: bearing : 0.5
range 1.000000

VALUES AFTER ERASING VARIABLE 2
Values with 2 values:
Value 1: (N5gtsam5Pose2E) (0, 0, 0)

Value 3: (N5gtsam6Point2E) (0, 0)

FACTOR GRAPH AFTER REPLACING POINT2 VARIABLE 2 WITH 3 IN FACTOR 1
NonlinearFactorGraph: size: 3

Factor 0: PriorFactor on 1
prior mean: (0, 0, 0)
isotropic dim=3 sigma=0.1

Factor 1: BearingRangeFactor
Factor 1: keys = { 1 3 }
isotropic dim=2 sigma=0.1
ExpressionFactor with measurement: bearing : 0.5
range 1.000000

Factor 2: BearingRangeFactor
Factor 2: keys = { 1 3 }
isotropic dim=2 sigma=0.1
ExpressionFactor with measurement: bearing : 0.5
range 1.000000

terminate called after throwing an instance of 'gtsam::ValuesKeyDoesNotExist'
what(): Attempting to at the key "2", which does not exist in the Values.

Process finished with exit code 134 (interrupted by signal 6: SIGABRT)

A workaround that suggests it is indeed a bug is to not use rekey, but insert a new factor, which does not cause this probem:

    auto test = boost::dynamic_pointer_cast<gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>>(factorgraph.at(1));
    gtsam::BearingRange<gtsam::Pose2,gtsam::Point2> bearingrange = test->measured();
    auto newfactor = boost::make_shared<gtsam::BearingRangeFactor<gtsam::Pose2,gtsam::Point2>>(1,3,bearingrange.bearing().theta(),bearingrange.range(),DummyNoise2);
    factorgraph.replace(1,  newfactor);

[dellaert]

Indeed. GTSAM is conceived as "functional": you create graphs, factors once, and you are not supposed to change them imperatively. A notable exception is iSAM, which does keep state, but it also will never re-key. I will close this as it is not a bug, and I think you already figured out the solution.