diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 63c512edb..b63e5faeb 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -251,10 +251,10 @@ void runIncremental() Key firstPose; while(nextMeasurement < datasetMeasurements.size()) { - if(BetweenFactor::shared_ptr measurement = + if(BetweenFactor::shared_ptr factor = boost::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) { - Key key1 = measurement->key1(), key2 = measurement->key2(); + Key key1 = factor->key1(), key2 = factor->key2(); if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { // We found an odometry starting at firstStep firstPose = std::min(key1, key2); @@ -302,52 +302,53 @@ void runIncremental() NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement]; - if(BetweenFactor::shared_ptr measurement = + if(BetweenFactor::shared_ptr factor = boost::dynamic_pointer_cast >(measurementf)) { // Stop collecting measurements that are for future steps - if(measurement->key1() > step || measurement->key2() > step) + if(factor->key1() > step || factor->key2() > step) break; // Require that one of the nodes is the current one - if(measurement->key1() != step && measurement->key2() != step) + if(factor->key1() != step && factor->key2() != step) throw runtime_error("Problem in data file, out-of-sequence measurements"); // Add a new factor - newFactors.push_back(measurement); + newFactors.push_back(factor); + const auto& measured = factor->measured(); // Initialize the new variable - if(measurement->key1() > measurement->key2()) { - if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry + if(factor->key1() > factor->key2()) { + if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry if(step == 1) - newVariables.insert(measurement->key1(), measurement->measured().inverse()); + newVariables.insert(factor->key1(), measured.inverse()); else { - Pose prevPose = isam2.calculateEstimate(measurement->key2()); - newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse()); + Pose prevPose = isam2.calculateEstimate(factor->key2()); + newVariables.insert(factor->key1(), prevPose * measured.inverse()); } } } else { - if(!newVariables.exists(measurement->key2())) { // Only need to check newVariables since loop closures come after odometry + if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry if(step == 1) - newVariables.insert(measurement->key2(), measurement->measured()); + newVariables.insert(factor->key2(), measured); else { - Pose prevPose = isam2.calculateEstimate(measurement->key1()); - newVariables.insert(measurement->key2(), prevPose * measurement->measured()); + Pose prevPose = isam2.calculateEstimate(factor->key1()); + newVariables.insert(factor->key2(), prevPose * measured); } } } } - else if(BearingRangeFactor::shared_ptr measurement = + else if(BearingRangeFactor::shared_ptr factor = boost::dynamic_pointer_cast >(measurementf)) { - Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1]; + Key poseKey = factor->keys()[0], lmKey = factor->keys()[1]; // Stop collecting measurements that are for future steps if(poseKey > step) throw runtime_error("Problem in data file, out-of-sequence measurements"); // Add new factor - newFactors.push_back(measurement); + newFactors.push_back(factor); // Initialize new landmark if(!isam2.getLinearizationPoint().exists(lmKey)) @@ -357,8 +358,9 @@ void runIncremental() pose = isam2.calculateEstimate(poseKey); else pose = newVariables.at(poseKey); - Rot2 measuredBearing = measurement->measured().first; - double measuredRange = measurement->measured().second; + const auto& measured = factor->measured(); + Rot2 measuredBearing = measured.bearing(); + double measuredRange = measured.range(); newVariables.insert(lmKey, pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0)))); } diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 9de5a07d8..ffa373027 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -67,6 +67,12 @@ public: /// @name Standard Interface /// @{ + /// Return bearing measurement + const B& bearing() const { return bearing_; } + + /// Return range measurement + const R& range() const { return range_; } + /// Prediction function that stacks measurements static BearingRange Measure( const A1& a1, const A2& a2,