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

test SmartFactor when body_P_sensor is passed in

release/4.3a0
Varun Agrawal 2020-07-07 19:24:38 -04:00
parent 55f686dd38
commit 73007fe048
2 changed files with 41 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/slam/SmartFactorBase.h
View File

@ -206,11 +206,12 @@ protected:
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();
for (size_t i = 0; i < Fs->size(); i++) {
const Pose3 w_Pose_body = cameras[i].pose() * sensor_P_body;
const Pose3 world_T_body = cameras[i].pose() * sensor_T_body;
Matrix J(6, 6);
const Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
// Call compose to compute Jacobian
world_T_body.compose(*body_P_sensor_, J);
Fs->at(i) = Fs->at(i) * J;
}
}

42
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -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());
@ -60,6 +60,38 @@ 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_T_body = Pose3();
// Camera coordinates in world frame.
Pose3 wTc = world_T_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));
EXPECT(assert_equal(expectedFs, Fs[0])); // We only have the jacobian for the 1 camera
EXPECT(assert_equal(expectedE, E));
}
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>