Test body_P_sensor in SmartFactor #379
Conversation
gtsam/slam/SmartFactorBase.h
Outdated
| @@ -206,11 +206,12 @@ class SmartFactorBase: public NonlinearFactor { | |||
| boost::optional<Matrix&> E = boost::none) const { | |||
| Vector ue = cameras.reprojectionError(point, measured_, Fs, E); | |||
| if (body_P_sensor_ && Fs) { | |||
| const Pose3 sensor_P_body = body_P_sensor_->inverse(); | |||
| const Pose3 sensor_T_body = body_P_sensor_->inverse(); | |||
There was a problem hiding this comment.
Please keep P, as the field uses that convention
There was a problem hiding this comment.
Gotcha. I was trying to follow the convention as per Samarth's post. Reverted.
|
@dellaert pretty much. The original issue asked for a test so decided it was easy enough to do so (aka unit tests are my friends). |
|
So unit testing won this round. Discovered a bug in the handling of the jacobian for cases such as |
gtsam/slam/SmartFactorBase.h
Outdated
| @@ -207,10 +207,17 @@ class SmartFactorBase: public NonlinearFactor { | |||
| 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); | |||
gtsam/slam/SmartFactorBase.h
Outdated
| Matrix J(6, 6); | ||
| const Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); | ||
| const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; | ||
| Matrix J = Matrix::Zero(camera_dim, camera_dim); |
There was a problem hiding this comment.
Why switch to dynamic? Make static, use setZero(), save a malloc
There was a problem hiding this comment.
Gotcha. It was already dynamic for some reason so was trying not to inadvertently introduce any bugs.
gtsam/slam/SmartFactorBase.h
Outdated
| 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; |
There was a problem hiding this comment.
You can just pass the block into the optionalJacobian slot. Amazing, right?
There was a problem hiding this comment.
I tried doing that but it was complaining about something (which I don't remember right now). I am guessing it will fix itself with static matrices.
There was a problem hiding this comment.
So as per this link, casting an Eigen::Block expression to a non-const Matrix is not possible.
Quoting the specific line:
When trying to execute the following code
MatrixXf C = MatrixXf::Zero(3,6); cov(x,y, C.block(0,0,3,3));the compiler will fail, because it is not possible to convert the expression returned by MatrixXf::block() into a non-const MatrixXf&. This is the case because the compiler wants to protect you from writing your result to a temporary object.
There was a problem hiding this comment.
Is the same true for fixed matrices, though?
There was a problem hiding this comment.
Looks like it. I am having compile time issues otherwise, specifically signature mismatch. This is the best explanation since we're trying to convert a block expression to a non-const matrix as a requirement.
Addresses #20.
This change is