From 6ae3b80baec64bf666112e5b8f9955538a4ac99a Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 3 Apr 2021 18:10:39 -0400 Subject: [PATCH] fixed glitch highlighted by CI --- .../slam/SmartStereoProjectionFactorPP.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp index 52e3bb4cc..07344ab04 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp @@ -44,18 +44,18 @@ void SmartStereoProjectionFactorPP::add( void SmartStereoProjectionFactorPP::add( const std::vector& measurements, - const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, + const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, const std::vector>& Ks) { - assert(measurements.size() == poseKeys.size()); - assert(w_P_body_keys.size() == body_P_cam_keys.size()); - assert(w_P_body_keys.size() == Ks.size()); + assert(world_P_body_keys.size() == measurements.size()); + assert(world_P_body_keys.size() == body_P_cam_keys.size()); + assert(world_P_body_keys.size() == Ks.size()); for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements[i], w_P_body_keys[i]); + Base::add(measurements[i], world_P_body_keys[i]); // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) keys_.push_back(body_P_cam_keys[i]); // add only unique keys - world_P_body_keys_.push_back(w_P_body_keys[i]); + world_P_body_keys_.push_back(world_P_body_keys[i]); body_P_cam_keys_.push_back(body_P_cam_keys[i]); K_all_.push_back(Ks[i]); @@ -64,17 +64,17 @@ void SmartStereoProjectionFactorPP::add( void SmartStereoProjectionFactorPP::add( const std::vector& measurements, - const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, + const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, const boost::shared_ptr& K) { - assert(poseKeys.size() == measurements.size()); - assert(w_P_body_keys.size() == body_P_cam_keys.size()); + assert(world_P_body_keys.size() == measurements.size()); + assert(world_P_body_keys.size() == body_P_cam_keys.size()); for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements[i], w_P_body_keys[i]); + Base::add(measurements[i], world_P_body_keys[i]); // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) keys_.push_back(body_P_cam_keys[i]); // add only unique keys - world_P_body_keys_.push_back(w_P_body_keys[i]); + world_P_body_keys_.push_back(world_P_body_keys[i]); body_P_cam_keys_.push_back(body_P_cam_keys[i]); K_all_.push_back(K);