all tests work except serialization
lcarlone
parent
fa4de18742
commit
3758fdaa5d
|
|
@ -38,7 +38,7 @@ namespace gtsam {
|
|||
/**
|
||||
* This factor assumes that camera calibration is fixed (but each camera
|
||||
* measurement can have a different extrinsic and intrinsic calibration).
|
||||
* The factor only constrains poses (variable dimension is 6).
|
||||
* The factor only constrains poses (variable dimension is 6 for each pose).
|
||||
* This factor requires that values contains the involved poses (Pose3).
|
||||
* If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead!
|
||||
* If the calibration should be optimized, as well, use SmartProjectionFactor instead!
|
||||
|
|
@ -60,7 +60,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
/// vector of keys (one for each observation) with potentially repeated keys
|
||||
KeyVector nonUniqueKeys_;
|
||||
|
||||
/// cameras in the rig (fixed poses wrt body + fixed intrinsics)
|
||||
/// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera)
|
||||
typename Base::Cameras cameraRig_;
|
||||
|
||||
/// vector of camera Ids (one for each observation), identifying which camera took the measurement
|
||||
|
|
@ -185,13 +185,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
*/
|
||||
typename Base::Cameras cameras(const Values& values) const override {
|
||||
typename Base::Cameras cameras;
|
||||
cameras.reserve(nonUniqueKeys_.size()); // preallocate
|
||||
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
|
||||
const Key cameraId = cameraIds_[i];
|
||||
const Pose3& body_P_cam_i = cameraRig_[cameraId].pose();
|
||||
const Pose3 world_P_sensor_i = values.at<Pose3>(nonUniqueKeys_[i])
|
||||
* body_P_cam_i;
|
||||
* cameraRig_[ cameraIds_[i] ].pose(); // cameraRig_[ cameraIds_[i] ].pose() is body_P_cam_i
|
||||
cameras.emplace_back(world_P_sensor_i,
|
||||
make_shared<typename CAMERA::CalibrationType>(cameraRig_[cameraId].calibration()));
|
||||
make_shared<typename CAMERA::CalibrationType>(cameraRig_[ cameraIds_[i] ].calibration()));
|
||||
}
|
||||
return cameras;
|
||||
}
|
||||
|
|
@ -223,10 +222,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
} else { // valid result: compute jacobians
|
||||
b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
|
||||
for (size_t i = 0; i < Fs.size(); i++) {
|
||||
const Key cameraId = cameraIds_[i];
|
||||
const Pose3 body_P_sensor = cameraRig_[cameraId].pose();
|
||||
const Pose3 sensor_P_body = body_P_sensor.inverse();
|
||||
const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
|
||||
const Pose3& body_P_sensor = cameraRig_[ cameraIds_[i] ].pose();
|
||||
const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse();
|
||||
Eigen::Matrix<double, DimPose, DimPose> H;
|
||||
world_P_body.compose(body_P_sensor, H);
|
||||
Fs.at(i) = Fs.at(i) * H;
|
||||
|
|
@ -246,11 +243,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
Cameras cameras = this->cameras(values);
|
||||
|
||||
// Create structures for Hessian Factors
|
||||
FastVector<size_t> js;
|
||||
FastVector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
|
||||
FastVector < Vector > gs(nrUniqueKeys);
|
||||
std::vector<size_t> js;
|
||||
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
|
||||
std::vector < Vector > gs(nrUniqueKeys);
|
||||
|
||||
if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera
|
||||
if (this->measured_.size() != cameras.size()) // 1 observation per camera
|
||||
throw std::runtime_error("SmartProjectionRigFactor: "
|
||||
"measured_.size() inconsistent with input");
|
||||
|
||||
|
|
|
|||
|
|
@ -71,7 +71,7 @@ namespace vanillaPose {
|
|||
typedef PinholePose<Cal3_S2> Camera;
|
||||
typedef CameraSet<Camera> Cameras;
|
||||
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
|
||||
typedef SmartProjectionRigFactor<Camera> SmartFactorP;
|
||||
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
|
||||
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
|
||||
Camera level_camera(level_pose, sharedK);
|
||||
Camera level_camera_right(pose_right, sharedK);
|
||||
|
|
@ -86,7 +86,7 @@ namespace vanillaPose2 {
|
|||
typedef PinholePose<Cal3_S2> Camera;
|
||||
typedef CameraSet<Camera> Cameras;
|
||||
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
|
||||
typedef SmartProjectionRigFactor<Camera> SmartFactorP;
|
||||
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
|
||||
static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
|
||||
Camera level_camera(level_pose, sharedK2);
|
||||
Camera level_camera_right(pose_right, sharedK2);
|
||||
|
|
@ -99,6 +99,7 @@ Camera cam3(pose_above, sharedK2);
|
|||
// Cal3Bundler cameras
|
||||
namespace bundler {
|
||||
typedef PinholeCamera<Cal3Bundler> Camera;
|
||||
typedef CameraSet<Camera> Cameras;
|
||||
typedef SmartProjectionFactor<Camera> SmartFactor;
|
||||
static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
|
||||
Camera level_camera(level_pose, K);
|
||||
|
|
@ -115,8 +116,9 @@ typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
|||
// Cal3Bundler poses
|
||||
namespace bundlerPose {
|
||||
typedef PinholePose<Cal3Bundler> Camera;
|
||||
typedef CameraSet<Camera> Cameras;
|
||||
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
|
||||
typedef SmartProjectionRigFactor<Camera> SmartFactorP;
|
||||
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
|
||||
static boost::shared_ptr<Cal3Bundler> sharedBundlerK(
|
||||
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
|
||||
Camera level_camera(level_pose, sharedBundlerK);
|
||||
|
|
|
|||
|
|
@ -60,7 +60,7 @@ TEST( SmartProjectionRigFactor, Constructor) {
|
|||
using namespace vanillaPose;
|
||||
Cameras cameraRig;
|
||||
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig));
|
||||
SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -69,7 +69,7 @@ TEST( SmartProjectionRigFactor, Constructor2) {
|
|||
Cameras cameraRig;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactorP factor1(model, cameraRig, params);
|
||||
SmartRigFactor factor1(model, cameraRig, params);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -77,7 +77,7 @@ TEST( SmartProjectionRigFactor, Constructor3) {
|
|||
using namespace vanillaPose;
|
||||
Cameras cameraRig;
|
||||
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig));
|
||||
SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig));
|
||||
factor1->add(measurement1, x1, cameraId1);
|
||||
}
|
||||
|
||||
|
|
@ -88,7 +88,7 @@ TEST( SmartProjectionRigFactor, Constructor4) {
|
|||
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactorP factor1(model, cameraRig, params);
|
||||
SmartRigFactor factor1(model, cameraRig, params);
|
||||
factor1.add(measurement1, x1, cameraId1);
|
||||
}
|
||||
|
||||
|
|
@ -98,10 +98,10 @@ TEST( SmartProjectionRigFactor, Equals ) {
|
|||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
|
||||
SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig));
|
||||
SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig));
|
||||
factor1->add(measurement1, x1, cameraId1);
|
||||
|
||||
SmartFactorP::shared_ptr factor2(new SmartFactorP(model, cameraRig));
|
||||
SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig));
|
||||
factor2->add(measurement1, x1, cameraId1);
|
||||
|
||||
CHECK(assert_equal(*factor1, *factor2));
|
||||
|
|
@ -119,7 +119,7 @@ TEST( SmartProjectionRigFactor, noiseless ) {
|
|||
Point2 level_uv = level_camera.project(landmark1);
|
||||
Point2 level_uv_right = level_camera_right.project(landmark1);
|
||||
|
||||
SmartFactorP factor(model, cameraRig);
|
||||
SmartRigFactor factor(model, cameraRig);
|
||||
factor.add(level_uv, x1, cameraId1); // both taken from the same camera
|
||||
factor.add(level_uv_right, x2, cameraId1);
|
||||
|
||||
|
|
@ -131,13 +131,13 @@ TEST( SmartProjectionRigFactor, noiseless ) {
|
|||
double expectedError = 0.0;
|
||||
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
|
||||
|
||||
SmartFactorP::Cameras cameras = factor.cameras(values);
|
||||
SmartRigFactor::Cameras cameras = factor.cameras(values);
|
||||
double actualError2 = factor.totalReprojectionError(cameras);
|
||||
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
|
||||
|
||||
// Calculate expected derivative for point (easiest to check)
|
||||
std::function<Vector(Point3)> f = //
|
||||
std::bind(&SmartFactorP::whitenedError<Point3>, factor, cameras,
|
||||
std::bind(&SmartRigFactor::whitenedError<Point3>, factor, cameras,
|
||||
std::placeholders::_1);
|
||||
|
||||
// Calculate using computeEP
|
||||
|
|
@ -153,7 +153,7 @@ TEST( SmartProjectionRigFactor, noiseless ) {
|
|||
EXPECT(assert_equal(expectedE, actualE, 1e-7));
|
||||
|
||||
// Calculate using reprojectionError
|
||||
SmartFactorP::Cameras::FBlocks F;
|
||||
SmartRigFactor::Cameras::FBlocks F;
|
||||
Matrix E;
|
||||
Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
|
||||
EXPECT(assert_equal(expectedE, E, 1e-7));
|
||||
|
|
@ -162,7 +162,7 @@ TEST( SmartProjectionRigFactor, noiseless ) {
|
|||
|
||||
// Calculate using computeJacobians
|
||||
Vector b;
|
||||
SmartFactorP::FBlocks Fs;
|
||||
SmartRigFactor::FBlocks Fs;
|
||||
factor.computeJacobians(Fs, E, b, cameras, *point);
|
||||
double actualError3 = b.squaredNorm();
|
||||
EXPECT(assert_equal(expectedE, E, 1e-7));
|
||||
|
|
@ -188,14 +188,14 @@ TEST( SmartProjectionRigFactor, noisy ) {
|
|||
Point3(0.5, 0.1, 0.3));
|
||||
values.insert(x2, pose_right.compose(noise_pose));
|
||||
|
||||
SmartFactorP::shared_ptr factor(new SmartFactorP(model,cameraRig));
|
||||
SmartRigFactor::shared_ptr factor(new SmartRigFactor(model,cameraRig));
|
||||
factor->add(level_uv, x1, cameraId1);
|
||||
factor->add(level_uv_right, x2, cameraId1);
|
||||
|
||||
double actualError1 = factor->error(values);
|
||||
|
||||
// create other factor by passing multiple measurements
|
||||
SmartFactorP::shared_ptr factor2(new SmartFactorP(model,cameraRig));
|
||||
SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model,cameraRig));
|
||||
|
||||
Point2Vector measurements;
|
||||
measurements.push_back(level_uv);
|
||||
|
|
@ -244,13 +244,13 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
|
|||
params.setDegeneracyMode(IGNORE_DEGENERACY);
|
||||
params.setEnableEPI(false);
|
||||
|
||||
SmartFactorP smartFactor1(model, cameraRig, params);
|
||||
SmartRigFactor smartFactor1(model, cameraRig, params);
|
||||
smartFactor1.add(measurements_cam1, views, cameraIds);
|
||||
|
||||
SmartFactorP smartFactor2(model, cameraRig, params);
|
||||
SmartRigFactor smartFactor2(model, cameraRig, params);
|
||||
smartFactor2.add(measurements_cam2, views, cameraIds);
|
||||
|
||||
SmartFactorP smartFactor3(model, cameraRig, params);
|
||||
SmartRigFactor smartFactor3(model, cameraRig, params);
|
||||
smartFactor3.add(measurements_cam3, views, cameraIds);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -305,13 +305,13 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) {
|
|||
KeyVector views {x1,x2,x3};
|
||||
FastVector<size_t> cameraIds{0,0,0};// 3 measurements from the same camera in the rig
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,cameraRig));
|
||||
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig));
|
||||
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model,cameraRig));
|
||||
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model,cameraRig));
|
||||
smartFactor2->add(measurements_cam2, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model,cameraRig));
|
||||
SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model,cameraRig));
|
||||
smartFactor3->add(measurements_cam3, views, cameraIds);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -378,11 +378,11 @@ TEST( SmartProjectionRigFactor, Factors ) {
|
|||
KeyVector views { x1, x2 };
|
||||
FastVector<size_t> cameraIds { 0, 0 };
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP
|
||||
SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor
|
||||
> (model,cameraRig);
|
||||
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||
|
||||
SmartFactorP::Cameras cameras;
|
||||
SmartRigFactor::Cameras cameras;
|
||||
cameras.push_back(cam1);
|
||||
cameras.push_back(cam2);
|
||||
|
||||
|
|
@ -465,13 +465,13 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) {
|
|||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
FastVector<size_t> cameraIds { 0, 0, 0};
|
||||
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig));
|
||||
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig));
|
||||
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor2->add(measurements_cam2, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig));
|
||||
SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor3->add(measurements_cam3, views, cameraIds);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -533,13 +533,13 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) {
|
|||
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
FastVector<size_t> cameraIds { 0, 0, 0};
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params));
|
||||
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params));
|
||||
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params));
|
||||
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params));
|
||||
smartFactor2->add(measurements_cam2, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params));
|
||||
SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params));
|
||||
smartFactor3->add(measurements_cam3, views, cameraIds);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -599,16 +599,16 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
|
|||
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
FastVector<size_t> cameraIds { 0, 0, 0};
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params));
|
||||
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params));
|
||||
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params));
|
||||
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params));
|
||||
smartFactor2->add(measurements_cam2, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params));
|
||||
SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params));
|
||||
smartFactor3->add(measurements_cam3, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, cameraRig, params));
|
||||
SmartRigFactor::shared_ptr smartFactor4(new SmartRigFactor(model, cameraRig, params));
|
||||
smartFactor4->add(measurements_cam4, views, cameraIds);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -662,13 +662,13 @@ TEST( SmartProjectionRigFactor, CheckHessian) {
|
|||
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
FastVector<size_t> cameraIds { 0, 0, 0};
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default
|
||||
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default
|
||||
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default
|
||||
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default
|
||||
smartFactor2->add(measurements_cam2, views, cameraIds);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default
|
||||
SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default
|
||||
smartFactor3->add(measurements_cam3, views, cameraIds);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
|
|
@ -735,7 +735,7 @@ TEST( SmartProjectionRigFactor, Hessian ) {
|
|||
cameraRig.push_back( Camera(Pose3::identity(), sharedK2) );
|
||||
FastVector<size_t> cameraIds { 0, 0 };
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig));
|
||||
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
|
||||
|
|
@ -752,347 +752,340 @@ TEST( SmartProjectionRigFactor, Hessian ) {
|
|||
// check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
|
||||
// using namespace bundlerPose;
|
||||
// SmartProjectionParams params;
|
||||
// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
// SmartFactorP factor(model, params);
|
||||
// factor.add(measurement1, x1, sharedBundlerK);
|
||||
//}
|
||||
//
|
||||
///* *************************************************************************/
|
||||
//TEST( SmartProjectionRigFactor, Cal3Bundler ) {
|
||||
//
|
||||
// using namespace bundlerPose;
|
||||
//
|
||||
// // three landmarks ~5 meters in front of camera
|
||||
// Point3 landmark3(3, 0, 3.0);
|
||||
//
|
||||
// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
//
|
||||
// // Project three landmarks into three cameras
|
||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
//
|
||||
// KeyVector views { x1, x2, x3 };
|
||||
//
|
||||
// std::vector < boost::shared_ptr < Cal3Bundler >> sharedBundlerKs;
|
||||
// sharedBundlerKs.push_back(sharedBundlerK);
|
||||
// sharedBundlerKs.push_back(sharedBundlerK);
|
||||
// sharedBundlerKs.push_back(sharedBundlerK);
|
||||
//
|
||||
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model));
|
||||
// smartFactor1->add(measurements_cam1, views, sharedBundlerKs);
|
||||
//
|
||||
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model));
|
||||
// smartFactor2->add(measurements_cam2, views, sharedBundlerKs);
|
||||
//
|
||||
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model));
|
||||
// smartFactor3->add(measurements_cam3, views, sharedBundlerKs);
|
||||
//
|
||||
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
//
|
||||
// NonlinearFactorGraph graph;
|
||||
// graph.push_back(smartFactor1);
|
||||
// graph.push_back(smartFactor2);
|
||||
// graph.push_back(smartFactor3);
|
||||
// graph.addPrior(x1, cam1.pose(), noisePrior);
|
||||
// graph.addPrior(x2, cam2.pose(), noisePrior);
|
||||
//
|
||||
// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
|
||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
// Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
// Values values;
|
||||
// values.insert(x1, cam1.pose());
|
||||
// values.insert(x2, cam2.pose());
|
||||
// // initialize third pose with some noise, we expect it to move back to original pose_above
|
||||
// values.insert(x3, pose_above * noise_pose);
|
||||
// EXPECT(
|
||||
// assert_equal(
|
||||
// Pose3(
|
||||
// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
|
||||
// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
|
||||
// Point3(0.1, -0.1, 1.9)),
|
||||
// values.at<Pose3>(x3)));
|
||||
//
|
||||
// Values result;
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
// result = optimizer.optimize();
|
||||
// EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3), 1e-6));
|
||||
//}
|
||||
//
|
||||
//#include <gtsam/slam/ProjectionFactor.h>
|
||||
//typedef GenericProjectionFactor<Pose3, Point3> TestProjectionFactor;
|
||||
//static Symbol l0('L', 0);
|
||||
///* *************************************************************************/
|
||||
//TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) {
|
||||
// // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark
|
||||
// // at a single pose, a setup that occurs in multi-camera systems
|
||||
//
|
||||
// using namespace vanillaPose;
|
||||
// Point2Vector measurements_lmk1;
|
||||
//
|
||||
// // Project three landmarks into three cameras
|
||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
|
||||
//
|
||||
// // create redundant measurements:
|
||||
// Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
|
||||
// measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement
|
||||
//
|
||||
// // create inputs
|
||||
// std::vector<Key> keys;
|
||||
// keys.push_back(x1);
|
||||
// keys.push_back(x2);
|
||||
// keys.push_back(x3);
|
||||
// keys.push_back(x1);
|
||||
//
|
||||
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs;
|
||||
// sharedKs.push_back(sharedK);
|
||||
// sharedKs.push_back(sharedK);
|
||||
// sharedKs.push_back(sharedK);
|
||||
// sharedKs.push_back(sharedK);
|
||||
//
|
||||
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model));
|
||||
// smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs);
|
||||
//
|
||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
// Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
// Values values;
|
||||
// values.insert(x1, level_pose);
|
||||
// values.insert(x2, pose_right);
|
||||
// // initialize third pose with some noise to get a nontrivial linearization point
|
||||
// values.insert(x3, pose_above * noise_pose);
|
||||
// EXPECT( // check that the pose is actually noisy
|
||||
// assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
|
||||
//
|
||||
// // linearization point for the poses
|
||||
// Pose3 pose1 = level_pose;
|
||||
// Pose3 pose2 = pose_right;
|
||||
// Pose3 pose3 = pose_above * noise_pose;
|
||||
//
|
||||
// // ==== check Hessian of smartFactor1 =====
|
||||
// // -- compute actual Hessian
|
||||
// boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize(
|
||||
// values);
|
||||
// Matrix actualHessian = linearfactor1->information();
|
||||
//
|
||||
// // -- compute expected Hessian from manual Schur complement from Jacobians
|
||||
// // linearization point for the 3D point
|
||||
// smartFactor1->triangulateSafe(smartFactor1->cameras(values));
|
||||
// TriangulationResult point = smartFactor1->point();
|
||||
// EXPECT(point.valid()); // check triangulated point is valid
|
||||
//
|
||||
// // Use standard ProjectionFactor factor to calculate the Jacobians
|
||||
// Matrix F = Matrix::Zero(2 * 4, 6 * 3);
|
||||
// Matrix E = Matrix::Zero(2 * 4, 3);
|
||||
// Vector b = Vector::Zero(2 * 4);
|
||||
//
|
||||
// // create projection factors rolling shutter
|
||||
// TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0,
|
||||
// sharedK);
|
||||
// Matrix HPoseActual, HEActual;
|
||||
// // note: b is minus the reprojection error, cf the smart factor jacobian computation
|
||||
// b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual,
|
||||
// HEActual);
|
||||
// F.block<2, 6>(0, 0) = HPoseActual;
|
||||
// E.block<2, 3>(0, 0) = HEActual;
|
||||
//
|
||||
// TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0,
|
||||
// sharedK);
|
||||
// b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual,
|
||||
// HEActual);
|
||||
// F.block<2, 6>(2, 6) = HPoseActual;
|
||||
// E.block<2, 3>(2, 0) = HEActual;
|
||||
//
|
||||
// TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0,
|
||||
// sharedK);
|
||||
// b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual,
|
||||
// HEActual);
|
||||
// F.block<2, 6>(4, 12) = HPoseActual;
|
||||
// E.block<2, 3>(4, 0) = HEActual;
|
||||
//
|
||||
// TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0,
|
||||
// sharedK);
|
||||
// b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual,
|
||||
// HEActual);
|
||||
// F.block<2, 6>(6, 0) = HPoseActual;
|
||||
// E.block<2, 3>(6, 0) = HEActual;
|
||||
//
|
||||
// // whiten
|
||||
// F = (1 / sigma) * F;
|
||||
// E = (1 / sigma) * E;
|
||||
// b = (1 / sigma) * b;
|
||||
// //* G = F' * F - F' * E * P * E' * F
|
||||
// Matrix P = (E.transpose() * E).inverse();
|
||||
// Matrix expectedHessian = F.transpose() * F
|
||||
// - (F.transpose() * E * P * E.transpose() * F);
|
||||
// EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
|
||||
//
|
||||
// // ==== check Information vector of smartFactor1 =====
|
||||
// GaussianFactorGraph gfg;
|
||||
// gfg.add(linearfactor1);
|
||||
// Matrix actualHessian_v2 = gfg.hessian().first;
|
||||
// EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian
|
||||
//
|
||||
// // -- compute actual information vector
|
||||
// Vector actualInfoVector = gfg.hessian().second;
|
||||
//
|
||||
// // -- compute expected information vector from manual Schur complement from Jacobians
|
||||
// //* g = F' * (b - E * P * E' * b)
|
||||
// Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
|
||||
// EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
|
||||
//
|
||||
// // ==== check error of smartFactor1 (again) =====
|
||||
// NonlinearFactorGraph nfg_projFactors;
|
||||
// nfg_projFactors.add(factor11);
|
||||
// nfg_projFactors.add(factor12);
|
||||
// nfg_projFactors.add(factor13);
|
||||
// nfg_projFactors.add(factor14);
|
||||
// values.insert(l0, *point);
|
||||
//
|
||||
// double actualError = smartFactor1->error(values);
|
||||
// double expectedError = nfg_projFactors.error(values);
|
||||
// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
|
||||
//}
|
||||
//
|
||||
///* *************************************************************************/
|
||||
//TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) {
|
||||
//
|
||||
// using namespace vanillaPose;
|
||||
// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
|
||||
//
|
||||
// // Project three landmarks into three cameras
|
||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
|
||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
|
||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
|
||||
//
|
||||
// // create inputs
|
||||
// std::vector<Key> keys;
|
||||
// keys.push_back(x1);
|
||||
// keys.push_back(x2);
|
||||
// keys.push_back(x3);
|
||||
//
|
||||
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs;
|
||||
// sharedKs.push_back(sharedK);
|
||||
// sharedKs.push_back(sharedK);
|
||||
// sharedKs.push_back(sharedK);
|
||||
//
|
||||
// // For first factor, we create redundant measurement (taken by the same keys as factor 1, to
|
||||
// // make sure the redundancy in the keys does not create problems)
|
||||
// Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
|
||||
// measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement
|
||||
// std::vector<Key> keys_redundant = keys;
|
||||
// keys_redundant.push_back(keys.at(0)); // we readd the first key
|
||||
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs_redundant = sharedKs;
|
||||
// sharedKs_redundant.push_back(sharedK);// we readd the first calibration
|
||||
//
|
||||
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model));
|
||||
// smartFactor1->add(measurements_lmk1_redundant, keys_redundant, sharedKs_redundant);
|
||||
//
|
||||
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model));
|
||||
// smartFactor2->add(measurements_lmk2, keys, sharedKs);
|
||||
//
|
||||
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model));
|
||||
// smartFactor3->add(measurements_lmk3, keys, sharedKs);
|
||||
//
|
||||
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
//
|
||||
// NonlinearFactorGraph graph;
|
||||
// graph.push_back(smartFactor1);
|
||||
// graph.push_back(smartFactor2);
|
||||
// graph.push_back(smartFactor3);
|
||||
// graph.addPrior(x1, level_pose, noisePrior);
|
||||
// graph.addPrior(x2, pose_right, noisePrior);
|
||||
//
|
||||
// Values groundTruth;
|
||||
// groundTruth.insert(x1, level_pose);
|
||||
// groundTruth.insert(x2, pose_right);
|
||||
// groundTruth.insert(x3, pose_above);
|
||||
// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
|
||||
//
|
||||
// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
|
||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
// Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
// Values values;
|
||||
// values.insert(x1, level_pose);
|
||||
// values.insert(x2, pose_right);
|
||||
// // initialize third pose with some noise, we expect it to move back to original pose_above
|
||||
// values.insert(x3, pose_above * noise_pose);
|
||||
// EXPECT( // check that the pose is actually noisy
|
||||
// assert_equal(
|
||||
// Pose3(
|
||||
// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
|
||||
// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
|
||||
// Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
|
||||
//
|
||||
// Values result;
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
// result = optimizer.optimize();
|
||||
// EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
|
||||
//}
|
||||
//
|
||||
//#ifndef DISABLE_TIMING
|
||||
//#include <gtsam/base/timing.h>
|
||||
//// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor
|
||||
////-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0)
|
||||
////| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0)
|
||||
////| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0)
|
||||
///* *************************************************************************/
|
||||
//TEST( SmartProjectionRigFactor, timing ) {
|
||||
//
|
||||
// using namespace vanillaPose;
|
||||
//
|
||||
// // Default cameras for simple derivatives
|
||||
// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
|
||||
//
|
||||
// Rot3 R = Rot3::identity();
|
||||
// Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
|
||||
// Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
|
||||
// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
|
||||
// Pose3 body_P_sensorId = Pose3::identity();
|
||||
//
|
||||
// // one landmarks 1m in front of camera
|
||||
// Point3 landmark1(0, 0, 10);
|
||||
//
|
||||
// Point2Vector measurements_lmk1;
|
||||
//
|
||||
// // Project 2 landmarks into 2 cameras
|
||||
// measurements_lmk1.push_back(cam1.project(landmark1));
|
||||
// measurements_lmk1.push_back(cam2.project(landmark1));
|
||||
//
|
||||
// size_t nrTests = 1000;
|
||||
//
|
||||
// for(size_t i = 0; i<nrTests; i++){
|
||||
// SmartFactorP::shared_ptr smartFactorP(new SmartFactorP(model));
|
||||
// smartFactorP->add(measurements_lmk1[0], x1, sharedKSimple, body_P_sensorId);
|
||||
// smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId);
|
||||
//
|
||||
// Values values;
|
||||
// values.insert(x1, pose1);
|
||||
// values.insert(x2, pose2);
|
||||
// gttic_(SmartFactorP_LINEARIZE);
|
||||
// smartFactorP->linearize(values);
|
||||
// gttoc_(SmartFactorP_LINEARIZE);
|
||||
// }
|
||||
//
|
||||
// for(size_t i = 0; i<nrTests; i++){
|
||||
// SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple));
|
||||
// smartFactor->add(measurements_lmk1[0], x1);
|
||||
// smartFactor->add(measurements_lmk1[1], x2);
|
||||
//
|
||||
// Values values;
|
||||
// values.insert(x1, pose1);
|
||||
// values.insert(x2, pose2);
|
||||
// gttic_(SmartPoseFactor_LINEARIZE);
|
||||
// smartFactor->linearize(values);
|
||||
// gttoc_(SmartPoseFactor_LINEARIZE);
|
||||
// }
|
||||
// tictoc_print_();
|
||||
//}
|
||||
//#endif
|
||||
//
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
|
||||
using namespace bundlerPose;
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) );
|
||||
|
||||
SmartProjectionParams params;
|
||||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
SmartRigFactor factor(model, cameraRig, params);
|
||||
factor.add(measurement1, x1, cameraId1);
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionRigFactor, Cal3Bundler ) {
|
||||
|
||||
using namespace bundlerPose;
|
||||
|
||||
// three landmarks ~5 meters in front of camera
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
KeyVector views { x1, x2, x3 };
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) );
|
||||
FastVector<size_t> cameraIds { 0, 0, 0 };
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor2->add(measurements_cam2, views, cameraIds);
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor3->add(measurements_cam3, views, cameraIds);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, cam1.pose(), noisePrior);
|
||||
graph.addPrior(x2, cam2.pose(), noisePrior);
|
||||
|
||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, cam1.pose());
|
||||
values.insert(x2, cam2.pose());
|
||||
// initialize third pose with some noise, we expect it to move back to original pose_above
|
||||
values.insert(x3, pose_above * noise_pose);
|
||||
EXPECT(
|
||||
assert_equal(
|
||||
Pose3(
|
||||
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
|
||||
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
|
||||
Point3(0.1, -0.1, 1.9)),
|
||||
values.at<Pose3>(x3)));
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3), 1e-6));
|
||||
}
|
||||
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
typedef GenericProjectionFactor<Pose3, Point3> TestProjectionFactor;
|
||||
static Symbol l0('L', 0);
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) {
|
||||
// in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark
|
||||
// at a single pose, a setup that occurs in multi-camera systems
|
||||
|
||||
using namespace vanillaPose;
|
||||
Point2Vector measurements_lmk1;
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
|
||||
|
||||
// create redundant measurements:
|
||||
Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
|
||||
measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement
|
||||
|
||||
// create inputs
|
||||
std::vector<Key> keys { x1, x2, x3, x1};
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
FastVector<size_t> cameraIds { 0, 0, 0, 0 };
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds);
|
||||
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, level_pose);
|
||||
values.insert(x2, pose_right);
|
||||
// initialize third pose with some noise to get a nontrivial linearization point
|
||||
values.insert(x3, pose_above * noise_pose);
|
||||
EXPECT( // check that the pose is actually noisy
|
||||
assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
|
||||
|
||||
// linearization point for the poses
|
||||
Pose3 pose1 = level_pose;
|
||||
Pose3 pose2 = pose_right;
|
||||
Pose3 pose3 = pose_above * noise_pose;
|
||||
|
||||
// ==== check Hessian of smartFactor1 =====
|
||||
// -- compute actual Hessian
|
||||
boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize(
|
||||
values);
|
||||
Matrix actualHessian = linearfactor1->information();
|
||||
|
||||
// -- compute expected Hessian from manual Schur complement from Jacobians
|
||||
// linearization point for the 3D point
|
||||
smartFactor1->triangulateSafe(smartFactor1->cameras(values));
|
||||
TriangulationResult point = smartFactor1->point();
|
||||
EXPECT(point.valid()); // check triangulated point is valid
|
||||
|
||||
// Use standard ProjectionFactor factor to calculate the Jacobians
|
||||
Matrix F = Matrix::Zero(2 * 4, 6 * 3);
|
||||
Matrix E = Matrix::Zero(2 * 4, 3);
|
||||
Vector b = Vector::Zero(2 * 4);
|
||||
|
||||
// create projection factors rolling shutter
|
||||
TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0,
|
||||
sharedK);
|
||||
Matrix HPoseActual, HEActual;
|
||||
// note: b is minus the reprojection error, cf the smart factor jacobian computation
|
||||
b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual,
|
||||
HEActual);
|
||||
F.block<2, 6>(0, 0) = HPoseActual;
|
||||
E.block<2, 3>(0, 0) = HEActual;
|
||||
|
||||
TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0,
|
||||
sharedK);
|
||||
b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual,
|
||||
HEActual);
|
||||
F.block<2, 6>(2, 6) = HPoseActual;
|
||||
E.block<2, 3>(2, 0) = HEActual;
|
||||
|
||||
TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0,
|
||||
sharedK);
|
||||
b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual,
|
||||
HEActual);
|
||||
F.block<2, 6>(4, 12) = HPoseActual;
|
||||
E.block<2, 3>(4, 0) = HEActual;
|
||||
|
||||
TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0,
|
||||
sharedK);
|
||||
b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual,
|
||||
HEActual);
|
||||
F.block<2, 6>(6, 0) = HPoseActual;
|
||||
E.block<2, 3>(6, 0) = HEActual;
|
||||
|
||||
// whiten
|
||||
F = (1 / sigma) * F;
|
||||
E = (1 / sigma) * E;
|
||||
b = (1 / sigma) * b;
|
||||
//* G = F' * F - F' * E * P * E' * F
|
||||
Matrix P = (E.transpose() * E).inverse();
|
||||
Matrix expectedHessian = F.transpose() * F
|
||||
- (F.transpose() * E * P * E.transpose() * F);
|
||||
EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
|
||||
|
||||
// ==== check Information vector of smartFactor1 =====
|
||||
GaussianFactorGraph gfg;
|
||||
gfg.add(linearfactor1);
|
||||
Matrix actualHessian_v2 = gfg.hessian().first;
|
||||
EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian
|
||||
|
||||
// -- compute actual information vector
|
||||
Vector actualInfoVector = gfg.hessian().second;
|
||||
|
||||
// -- compute expected information vector from manual Schur complement from Jacobians
|
||||
//* g = F' * (b - E * P * E' * b)
|
||||
Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
|
||||
EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
|
||||
|
||||
// ==== check error of smartFactor1 (again) =====
|
||||
NonlinearFactorGraph nfg_projFactors;
|
||||
nfg_projFactors.add(factor11);
|
||||
nfg_projFactors.add(factor12);
|
||||
nfg_projFactors.add(factor13);
|
||||
nfg_projFactors.add(factor14);
|
||||
values.insert(l0, *point);
|
||||
|
||||
double actualError = smartFactor1->error(values);
|
||||
double expectedError = nfg_projFactors.error(values);
|
||||
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
|
||||
|
||||
// create inputs
|
||||
std::vector<Key> keys { x1, x2, x3 };
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
FastVector<size_t> cameraIds { 0, 0, 0 };
|
||||
FastVector<size_t> cameraIdsRedundant { 0, 0, 0, 0 };
|
||||
|
||||
// For first factor, we create redundant measurement (taken by the same keys as factor 1, to
|
||||
// make sure the redundancy in the keys does not create problems)
|
||||
Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
|
||||
measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement
|
||||
std::vector<Key> keys_redundant = keys;
|
||||
keys_redundant.push_back(keys.at(0)); // we readd the first key
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant);
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor2->add(measurements_lmk2, keys, cameraIds);
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig));
|
||||
smartFactor3->add(measurements_lmk3, keys, cameraIds);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, level_pose, noisePrior);
|
||||
graph.addPrior(x2, pose_right, noisePrior);
|
||||
|
||||
Values groundTruth;
|
||||
groundTruth.insert(x1, level_pose);
|
||||
groundTruth.insert(x2, pose_right);
|
||||
groundTruth.insert(x3, pose_above);
|
||||
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
|
||||
|
||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, level_pose);
|
||||
values.insert(x2, pose_right);
|
||||
// initialize third pose with some noise, we expect it to move back to original pose_above
|
||||
values.insert(x3, pose_above * noise_pose);
|
||||
EXPECT( // check that the pose is actually noisy
|
||||
assert_equal(
|
||||
Pose3(
|
||||
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
|
||||
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
|
||||
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
|
||||
}
|
||||
|
||||
#ifndef DISABLE_TIMING
|
||||
#include <gtsam/base/timing.h>
|
||||
// this factor is slightly slower (but comparable) to original SmartProjectionPoseFactor
|
||||
//-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0)
|
||||
//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 children, min: 0 max: 0)
|
||||
//| -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 times, 0.065103 wall, 0.06 children, min: 0 max: 0)
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionRigFactor, timing ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
// Default cameras for simple derivatives
|
||||
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
|
||||
|
||||
Rot3 R = Rot3::identity();
|
||||
Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
|
||||
Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
|
||||
Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
|
||||
Pose3 body_P_sensorId = Pose3::identity();
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back( Camera(body_P_sensorId, sharedKSimple) );
|
||||
|
||||
// one landmarks 1m in front of camera
|
||||
Point3 landmark1(0, 0, 10);
|
||||
|
||||
Point2Vector measurements_lmk1;
|
||||
|
||||
// Project 2 landmarks into 2 cameras
|
||||
measurements_lmk1.push_back(cam1.project(landmark1));
|
||||
measurements_lmk1.push_back(cam2.project(landmark1));
|
||||
|
||||
size_t nrTests = 10000;
|
||||
|
||||
for(size_t i = 0; i<nrTests; i++){
|
||||
SmartRigFactor::shared_ptr smartFactorP(new SmartRigFactor(model, cameraRig));
|
||||
smartFactorP->add(measurements_lmk1[0], x1, cameraId1);
|
||||
smartFactorP->add(measurements_lmk1[1], x1, cameraId1);
|
||||
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
gttic_(SmartRigFactor_LINEARIZE);
|
||||
smartFactorP->linearize(values);
|
||||
gttoc_(SmartRigFactor_LINEARIZE);
|
||||
}
|
||||
|
||||
for(size_t i = 0; i<nrTests; i++){
|
||||
SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple));
|
||||
smartFactor->add(measurements_lmk1[0], x1);
|
||||
smartFactor->add(measurements_lmk1[1], x2);
|
||||
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
gttic_(SmartPoseFactor_LINEARIZE);
|
||||
smartFactor->linearize(values);
|
||||
gttoc_(SmartPoseFactor_LINEARIZE);
|
||||
}
|
||||
tictoc_print_();
|
||||
}
|
||||
#endif
|
||||
|
||||
///* ************************************************************************* */
|
||||
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
|
||||
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
|
||||
|
|
@ -1107,7 +1100,11 @@ TEST( SmartProjectionRigFactor, Hessian ) {
|
|||
// using namespace gtsam::serializationTestHelpers;
|
||||
// SmartProjectionParams params;
|
||||
// params.setRankTolerance(rankTol);
|
||||
// SmartFactorP factor(model, params);
|
||||
//
|
||||
// Cameras cameraRig; // single camera in the rig
|
||||
// cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||
//
|
||||
// SmartRigFactor factor(model, cameraRig, params);
|
||||
//
|
||||
// EXPECT(equalsObj(factor));
|
||||
// EXPECT(equalsXML(factor));
|
||||
|
|
@ -1120,14 +1117,13 @@ TEST( SmartProjectionRigFactor, Hessian ) {
|
|||
//// using namespace gtsam::serializationTestHelpers;
|
||||
//// SmartProjectionParams params;
|
||||
//// params.setRankTolerance(rankTol);
|
||||
//// SmartFactorP factor(model, params);
|
||||
//// SmartRigFactor factor(model, params);
|
||||
////
|
||||
//// // insert some measurements
|
||||
//// KeyVector key_view;
|
||||
//// Point2Vector meas_view;
|
||||
//// std::vector<boost::shared_ptr<Cal3_S2>> sharedKs;
|
||||
////
|
||||
////
|
||||
//// key_view.push_back(Symbol('x', 1));
|
||||
//// meas_view.push_back(Point2(10, 10));
|
||||
//// sharedKs.push_back(sharedK);
|
||||
|
|
|
|||
|
|
@ -341,19 +341,21 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
this->keys_
|
||||
.size(); // note: by construction, keys_ only contains unique keys
|
||||
|
||||
typename Base::Cameras cameras = this->cameras(values);
|
||||
|
||||
// Create structures for Hessian Factors
|
||||
KeyVector js;
|
||||
std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
|
||||
std::vector<Vector> gs(nrUniqueKeys);
|
||||
|
||||
if (this->measured_.size() !=
|
||||
this->cameras(values).size()) // 1 observation per interpolated camera
|
||||
cameras.size()) // 1 observation per interpolated camera
|
||||
throw std::runtime_error(
|
||||
"SmartProjectionPoseFactorRollingShutter: "
|
||||
"measured_.size() inconsistent with input");
|
||||
|
||||
// triangulate 3D point at given linearization point
|
||||
this->triangulateSafe(this->cameras(values));
|
||||
this->triangulateSafe(cameras);
|
||||
|
||||
if (!this->result_) { // failed: return "empty/zero" Hessian
|
||||
if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue