diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index bb261a9c4..be1584195 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -123,18 +123,16 @@ public: */ typename Base::Cameras cameras(const Values& values) const { typename Base::Cameras cameras; - for(const Key& k: this->keys_) { - Pose3 pose = values.at(k); - if (Base::body_P_sensor_) - pose = pose.compose(*(Base::body_P_sensor_)); - - Camera camera(pose, K_); - cameras.push_back(camera); + for (const Key& k : this->keys_) { + const Pose3 world_P_sensor_k = + Base::body_P_sensor_ ? values.at(k) * *Base::body_P_sensor_ + : values.at(k); + cameras.emplace_back(world_P_sensor_k, K_); } return cameras; } -private: + private: /// Serialization function friend class boost::serialization::access;