12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

revert variable change

release/4.3a0
Varun Agrawal 2020-07-09 00:46:21 -04:00
parent 73007fe048
commit 0c199dd406
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/slam/SmartFactorBase.h
View File

@ -206,12 +206,12 @@ protected:
boost::optional<Matrix&> E = boost::none) const { boost::optional<Matrix&> E = boost::none) const {
Vector ue = cameras.reprojectionError(point, measured_, Fs, E); Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
if (body_P_sensor_ && Fs) { 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++) { 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); Matrix J(6, 6);
// Call compose to compute Jacobian // 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; Fs->at(i) = Fs->at(i) * J;
} }
} }