Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Test body_P_sensor in SmartFactor #379

Merged
merged 7 commits into from
Jul 10, 2020
Merged

Test body_P_sensor in SmartFactor #379

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
73007fe
test SmartFactor when body_P_sensor is passed in
varunagrawal Jul 7, 2020
0c199dd
revert variable change
varunagrawal Jul 9, 2020
b9264cf
Merge branch 'develop' into fix/smartfactor_body_P_sensor
varunagrawal Jul 9, 2020
7dfc799
reduced tolerance for checking jacobian
varunagrawal Jul 9, 2020
f23587f
Add indentation
varunagrawal Jul 10, 2020
60c88f6
Handle extrinsics and intrinsics for jacobian
varunagrawal Jul 10, 2020
7259f89
Use static matrix and constexpr
varunagrawal Jul 10, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions gtsam/geometry/PinholePose.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,9 +107,9 @@ class PinholeBaseK: public PinholeBase {

// If needed, apply chain rule
if (Dpose)
*Dpose = Dpi_pn * *Dpose;
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
*Dpoint = Dpi_pn * *Dpoint;

return pi;
}
Expand Down
13 changes: 10 additions & 3 deletions gtsam/slam/SmartFactorBase.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Use constexpr int, no casting

size_t pose_dim = size_t(traits<Pose3>::dimension);

for (size_t i = 0; i < Fs->size(); i++) {
const Pose3 w_Pose_body = cameras[i].pose() * sensor_P_body;
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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why switch to dynamic? Make static, use setZero(), save a malloc

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Gotcha. It was already dynamic for some reason so was trying not to inadvertently introduce any bugs.

// 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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You can just pass the block into the optionalJacobian slot. Amazing, right?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is the same true for fixed matrices, though?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

eigen_error

Fs->at(i) = Fs->at(i) * J;
}
}
Expand Down
44 changes: 39 additions & 5 deletions gtsam/slam/tests/testSmartFactorBase.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,11 +37,11 @@ class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor() {}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
return 0.0;
}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10)
: Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
virtual double error(const Values& values) const { return 0.0; }
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
Expand All @@ -60,6 +60,40 @@ TEST(SmartFactorBase, Pinhole) {
EXPECT_LONGS_EQUAL(2 * 2, f.dim());
}

TEST(SmartFactorBase, PinholeWithSensor) {
Pose3 body_P_sensor(Rot3(), Point3(1, 0, 0));
PinholeFactor f = PinholeFactor(unit2, body_P_sensor);
EXPECT(assert_equal<Pose3>(f.body_P_sensor(), body_P_sensor));

PinholeFactor::Cameras cameras;
// Assume body at origin.
Pose3 world_P_body = Pose3();
// Camera coordinates in world frame.
Pose3 wTc = world_P_body * body_P_sensor;
cameras.push_back(PinholeCamera<Cal3Bundler>(wTc));

// Simple point to project slightly off image center
Point3 p(0, 0, 10);
Point2 measurement = cameras[0].project(p);
f.add(measurement, 1);

PinholeFactor::Cameras::FBlocks Fs;
Matrix E;
Vector error = f.unwhitenedError<Point3>(cameras, p, Fs, E);

Vector expectedError = Vector::Zero(2);
Matrix29 expectedFs;
expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0;
Matrix23 expectedE;
expectedE << 0.1, 0, 0.01, 0, 0.1, 0;

EXPECT(assert_equal(error, expectedError));
// We only have the jacobian for the 1 camera
// Use of a lower tolerance value due to compiler precision mismatch.
EXPECT(assert_equal(expectedFs, Fs[0], 1e-3));
EXPECT(assert_equal(expectedE, E));
}

/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>

Expand Down