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

Use static matrix and constexpr

release/4.3a0
Varun Agrawal 2020-07-10 09:26:48 -04:00
parent 60c88f67e9
commit 7259f899d9
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/slam/SmartFactorBase.h
View File

@ -207,17 +207,18 @@ protected:
Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
if (body_P_sensor_ && Fs) {
const Pose3 sensor_P_body = body_P_sensor_->inverse();
size_t camera_dim = size_t(traits<CAMERA>::dimension);
size_t pose_dim = size_t(traits<Pose3>::dimension);
constexpr int camera_dim = traits<CAMERA>::dimension;
constexpr int pose_dim = traits<Pose3>::dimension;
for (size_t i = 0; i < Fs->size(); i++) {
const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
Matrix J = Matrix::Zero(camera_dim, camera_dim);
Eigen::Matrix<double, camera_dim, camera_dim> J;
J.setZero();
Eigen::Matrix<double, pose_dim, pose_dim> H;
// Call compose to compute Jacobian for camera extrinsics
Matrix H(pose_dim, pose_dim);
world_P_body.compose(*body_P_sensor_, H);
// Assign extrinsics
J.block(0, 0, pose_dim, pose_dim) = H;
// Assign extrinsics part of the Jacobian
J.template block<pose_dim, pose_dim>(0, 0) = H;
Fs->at(i) = Fs->at(i) * J;
}
}