diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 3bedf599f..c9b510605 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -206,12 +206,12 @@ protected: boost::optional E = boost::none) const { Vector ue = cameras.reprojectionError(point, measured_, Fs, E); if (body_P_sensor_ && Fs) { - const Pose3 sensor_T_body = body_P_sensor_->inverse(); + const Pose3 sensor_P_body = body_P_sensor_->inverse(); for (size_t i = 0; i < Fs->size(); i++) { - const Pose3 world_T_body = cameras[i].pose() * sensor_T_body; + const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; Matrix J(6, 6); // Call compose to compute Jacobian - world_T_body.compose(*body_P_sensor_, J); + world_P_body.compose(*body_P_sensor_, J); Fs->at(i) = Fs->at(i) * J; } }