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

fixed formatting (plus small fix: std::vector -> fastVector)

release/4.3a0
lcarlone 2021-11-06 18:05:58 -04:00
parent 737dcf65e4
commit c4cd2b5080
2 changed files with 350 additions and 296 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

311
gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

@ -19,15 +19,17 @@
* @date August 2021 * @date August 2021
*/ */
#include "smartFactorScenarios.h" #include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h> #include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <boost/assign/std/map.hpp> #include <boost/assign/std/map.hpp>
#include <iostream> #include <iostream>
#include "smartFactorScenarios.h"
using namespace boost::assign; using namespace boost::assign;
using namespace std::placeholders; using namespace std::placeholders;
@ -37,8 +39,8 @@ static const double sigma = 0.1;
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
// Convenience for named keys // Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
using symbol_shorthand::X;
// tests data // tests data
static Symbol x1('X', 1); static Symbol x1('X', 1);
@ -115,7 +117,8 @@ TEST( SmartProjectionRigFactor, Equals ) {
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
SmartRigFactor::shared_ptr factor3(new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); SmartRigFactor::shared_ptr factor3(
new SmartRigFactor(model, Camera(Pose3::identity(), sharedK)));
factor3->add(measurement1, x1); // now use default factor3->add(measurement1, x1); // now use default
CHECK(assert_equal(*factor1, *factor3)); CHECK(assert_equal(*factor1, *factor3));
@ -123,7 +126,6 @@ TEST( SmartProjectionRigFactor, Equals ) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, noiseless) { TEST(SmartProjectionRigFactor, noiseless) {
using namespace vanillaPose; using namespace vanillaPose;
// Project two landmarks into two cameras // Project two landmarks into two cameras
@ -182,7 +184,6 @@ TEST( SmartProjectionRigFactor, noiseless ) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, noisy) { TEST(SmartProjectionRigFactor, noisy) {
using namespace vanillaPose; using namespace vanillaPose;
Cameras cameraRig; // single camera in the rig Cameras cameraRig; // single camera in the rig
@ -195,8 +196,8 @@ TEST( SmartProjectionRigFactor, noisy ) {
Values values; Values values;
values.insert(x1, cam1.pose()); values.insert(x1, cam1.pose());
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Pose3 noise_pose =
Point3(0.5, 0.1, 0.3)); Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3));
values.insert(x2, pose_right.compose(noise_pose)); values.insert(x2, pose_right.compose(noise_pose));
SmartRigFactor::shared_ptr factor(new SmartRigFactor(model, cameraRig)); SmartRigFactor::shared_ptr factor(new SmartRigFactor(model, cameraRig));
@ -225,8 +226,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
using namespace vanillaPose; using namespace vanillaPose;
// create arbitrary body_T_sensor (transforms from sensor to body) // create arbitrary body_T_sensor (transforms from sensor to body)
Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Pose3 body_T_sensor =
Point3(1, 1, 1)); Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
Cameras cameraRig; // single camera in the rig Cameras cameraRig; // single camera in the rig
cameraRig.push_back(Camera(body_T_sensor, sharedK)); cameraRig.push_back(Camera(body_T_sensor, sharedK));
@ -256,7 +257,9 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
params.setEnableEPI(false); params.setEnableEPI(false);
SmartRigFactor smartFactor1(model, cameraRig, params); SmartRigFactor smartFactor1(model, cameraRig, params);
smartFactor1.add(measurements_cam1, views); // use default CameraIds since we have a single camera smartFactor1.add(
measurements_cam1,
views); // use default CameraIds since we have a single camera
SmartRigFactor smartFactor2(model, cameraRig, params); SmartRigFactor smartFactor2(model, cameraRig, params);
smartFactor2.add(measurements_cam2, views); smartFactor2.add(measurements_cam2, views);
@ -283,8 +286,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
double expectedError = 0.0; double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-7) DOUBLES_EQUAL(expectedError, actualError, 1e-7)
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose =
Point3(0.1, 0.1, 0.1)); Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1));
Values values; Values values;
values.insert(x1, wTb1); values.insert(x1, wTb1);
values.insert(x2, wTb2); values.insert(x2, wTb2);
@ -304,10 +307,10 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
using namespace vanillaPose; using namespace vanillaPose;
// create arbitrary body_T_sensor (transforms from sensor to body) // create arbitrary body_T_sensor (transforms from sensor to body)
Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Pose3 body_T_sensor1 =
Point3(1, 1, 1)); Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Pose3 body_T_sensor2 =
Point3(0, 0, 1)); Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 body_T_sensor3 = Pose3::identity(); Pose3 body_T_sensor3 = Pose3::identity();
Cameras cameraRig; // single camera in the rig Cameras cameraRig; // single camera in the rig
@ -370,8 +373,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
double expectedError = 0.0; double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-7) DOUBLES_EQUAL(expectedError, actualError, 1e-7)
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose =
Point3(0.1, 0.1, 0.1)); Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1));
Values values; Values values;
values.insert(x1, wTb1); values.insert(x1, wTb1);
values.insert(x2, wTb2); values.insert(x2, wTb2);
@ -388,7 +391,6 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
using namespace vanillaPose2; using namespace vanillaPose2;
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -401,7 +403,8 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
KeyVector views{x1, x2, x3}; KeyVector views{x1, x2, x3};
FastVector<size_t> cameraIds{0,0,0};// 3 measurements from the same camera in the rig FastVector<size_t> cameraIds{
0, 0, 0}; // 3 measurements from the same camera in the rig
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig));
smartFactor1->add(measurements_cam1, views, cameraIds); smartFactor1->add(measurements_cam1, views, cameraIds);
@ -427,18 +430,18 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) {
groundTruth.insert(x3, cam3.pose()); groundTruth.insert(x3, cam3.pose());
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); 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/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), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, cam1.pose()); values.insert(x1, cam1.pose());
values.insert(x2, cam2.pose()); values.insert(x2, cam2.pose());
// initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
EXPECT( EXPECT(assert_equal(
assert_equal( Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), Point3(0.1, -0.1, 1.9)),
values.at<Pose3>(x3))); values.at<Pose3>(x3)));
@ -451,14 +454,13 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, Factors) { TEST(SmartProjectionRigFactor, Factors) {
using namespace vanillaPose; using namespace vanillaPose;
// Default cameras for simple derivatives // Default cameras for simple derivatives
Rot3 R; Rot3 R;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0));
Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK),
Pose3(R, Point3(1, 0, 0)), sharedK); cam2(Pose3(R, Point3(1, 0, 0)), sharedK);
// one landmarks 1m in front of camera // one landmarks 1m in front of camera
Point3 landmark1(0, 0, 10); Point3 landmark1(0, 0, 10);
@ -473,9 +475,10 @@ TEST( SmartProjectionRigFactor, Factors ) {
KeyVector views{x1, x2}; KeyVector views{x1, x2};
FastVector<size_t> cameraIds{0, 0}; FastVector<size_t> cameraIds{0, 0};
SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
> (model, Camera(Pose3::identity(), sharedK)); model, Camera(Pose3::identity(), sharedK));
smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds smartFactor1->add(measurements_cam1,
views); // we have a single camera so use default cameraIds
SmartRigFactor::Cameras cameras; SmartRigFactor::Cameras cameras;
cameras.push_back(cam1); cameras.push_back(cam1);
@ -501,7 +504,8 @@ TEST( SmartProjectionRigFactor, Factors ) {
perturbedDelta.insert(x2, delta); perturbedDelta.insert(x2, delta);
double expectedError = 2500; double expectedError = 2500;
// After eliminating the point, A1 and A2 contain 2-rank information on cameras: // After eliminating the point, A1 and A2 contain 2-rank information on
// cameras:
Matrix16 A1, A2; Matrix16 A1, A2;
A1 << -10, 0, 0, 0, 1, 0; A1 << -10, 0, 0, 0, 1, 0;
A2 << 10, 0, 1, 0, -1, 0; A2 << 10, 0, 1, 0, -1, 0;
@ -528,8 +532,8 @@ TEST( SmartProjectionRigFactor, Factors ) {
values.insert(x1, cam1.pose()); values.insert(x1, cam1.pose());
values.insert(x2, cam2.pose()); values.insert(x2, cam2.pose());
boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 boost::shared_ptr<RegularHessianFactor<6>> actual =
->createHessianFactor(values, 0.0); smartFactor1->createHessianFactor(values, 0.0);
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
EXPECT(assert_equal(expected, *actual, 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6));
EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6);
@ -539,7 +543,6 @@ TEST( SmartProjectionRigFactor, Factors ) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
using namespace vanillaPose; using namespace vanillaPose;
KeyVector views{x1, x2, x3}; KeyVector views{x1, x2, x3};
@ -578,20 +581,19 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) {
graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.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/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), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, cam1.pose()); values.insert(x1, cam1.pose());
values.insert(x2, cam2.pose()); values.insert(x2, cam2.pose());
// initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
EXPECT( EXPECT(assert_equal(Pose3(Rot3(1.11022302e-16, -0.0314107591, 0.99950656,
assert_equal( -0.99950656, -0.0313952598, -0.000986635786,
Pose3( 0.0314107591, -0.999013364, -0.0313952598),
Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656,
-0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
-0.0313952598),
Point3(0.1, -0.1, 1.9)), Point3(0.1, -0.1, 1.9)),
values.at<Pose3>(x3))); values.at<Pose3>(x3)));
@ -603,7 +605,6 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, landmarkDistance) { TEST(SmartProjectionRigFactor, landmarkDistance) {
using namespace vanillaPose; using namespace vanillaPose;
double excludeLandmarksFutherThanDist = 2; double excludeLandmarksFutherThanDist = 2;
@ -628,13 +629,16 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) {
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params));
smartFactor1->add(measurements_cam1, views, cameraIds); smartFactor1->add(measurements_cam1, views, cameraIds);
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); SmartRigFactor::shared_ptr smartFactor2(
new SmartRigFactor(model, cameraRig, params));
smartFactor2->add(measurements_cam2, views, cameraIds); smartFactor2->add(measurements_cam2, views, cameraIds);
SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); SmartRigFactor::shared_ptr smartFactor3(
new SmartRigFactor(model, cameraRig, params));
smartFactor3->add(measurements_cam3, views, cameraIds); smartFactor3->add(measurements_cam3, views, cameraIds);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -646,7 +650,8 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) {
graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.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/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), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
@ -663,11 +668,11 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
using namespace vanillaPose; using namespace vanillaPose;
double excludeLandmarksFutherThanDist = 1e10; double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error double dynamicOutlierRejectionThreshold =
1; // max 1 pixel of average reprojection error
KeyVector views{x1, x2, x3}; KeyVector views{x1, x2, x3};
@ -682,7 +687,8 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier measurements_cam4.at(0) =
measurements_cam4.at(0) + Point2(10, 10); // add outlier
SmartProjectionParams params; SmartProjectionParams params;
params.setLinearizationMode(gtsam::HESSIAN); params.setLinearizationMode(gtsam::HESSIAN);
@ -694,16 +700,20 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params));
smartFactor1->add(measurements_cam1, views, cameraIds); smartFactor1->add(measurements_cam1, views, cameraIds);
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); SmartRigFactor::shared_ptr smartFactor2(
new SmartRigFactor(model, cameraRig, params));
smartFactor2->add(measurements_cam2, views, cameraIds); smartFactor2->add(measurements_cam2, views, cameraIds);
SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); SmartRigFactor::shared_ptr smartFactor3(
new SmartRigFactor(model, cameraRig, params));
smartFactor3->add(measurements_cam3, views, cameraIds); smartFactor3->add(measurements_cam3, views, cameraIds);
SmartRigFactor::shared_ptr smartFactor4(new SmartRigFactor(model, cameraRig, params)); SmartRigFactor::shared_ptr smartFactor4(
new SmartRigFactor(model, cameraRig, params));
smartFactor4->add(measurements_cam4, views, cameraIds); smartFactor4->add(measurements_cam4, views, cameraIds);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -730,14 +740,13 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, CheckHessian) { TEST(SmartProjectionRigFactor, CheckHessian) {
KeyVector views{x1, x2, x3}; KeyVector views{x1, x2, x3};
using namespace vanillaPose; using namespace vanillaPose;
// Two slightly different cameras // Two slightly different cameras
Pose3 pose2 = level_pose Pose3 pose2 =
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Camera cam2(pose2, sharedK); Camera cam2(pose2, sharedK);
Camera cam3(pose3, sharedK); Camera cam3(pose3, sharedK);
@ -757,13 +766,16 @@ TEST( SmartProjectionRigFactor, CheckHessian) {
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default
smartFactor1->add(measurements_cam1, views, cameraIds); smartFactor1->add(measurements_cam1, views, cameraIds);
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default SmartRigFactor::shared_ptr smartFactor2(
new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default
smartFactor2->add(measurements_cam2, views, cameraIds); smartFactor2->add(measurements_cam2, views, cameraIds);
SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default SmartRigFactor::shared_ptr smartFactor3(
new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default
smartFactor3->add(measurements_cam3, views, cameraIds); smartFactor3->add(measurements_cam3, views, cameraIds);
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -771,20 +783,19 @@ TEST( SmartProjectionRigFactor, CheckHessian) {
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
// 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/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), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, cam1.pose()); values.insert(x1, cam1.pose());
values.insert(x2, cam2.pose()); values.insert(x2, cam2.pose());
// initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose3 * noise_pose); values.insert(x3, pose3 * noise_pose);
EXPECT( EXPECT(assert_equal(Pose3(Rot3(0.00563056869, -0.130848107, 0.991386438,
assert_equal( -0.991390265, -0.130426831, -0.0115837907,
Pose3( 0.130819108, -0.98278564, -0.130455917),
Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
-0.130426831, -0.0115837907, 0.130819108, -0.98278564,
-0.130455917),
Point3(0.0897734171, -0.110201006, 0.901022872)), Point3(0.0897734171, -0.110201006, 0.901022872)),
values.at<Pose3>(x3))); values.at<Pose3>(x3)));
@ -792,21 +803,23 @@ TEST( SmartProjectionRigFactor, CheckHessian) {
boost::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values); boost::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
boost::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values); boost::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values);
Matrix CumulativeInformation = factor1->information() + factor2->information() Matrix CumulativeInformation =
+ factor3->information(); factor1->information() + factor2->information() + factor3->information();
boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize( boost::shared_ptr<GaussianFactorGraph> GaussianGraph =
values); graph.linearize(values);
Matrix GraphInformation = GaussianGraph->hessian().first; Matrix GraphInformation = GaussianGraph->hessian().first;
// Check Hessian // Check Hessian
EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));
Matrix AugInformationMatrix = factor1->augmentedInformation() Matrix AugInformationMatrix = factor1->augmentedInformation() +
+ factor2->augmentedInformation() + factor3->augmentedInformation(); factor2->augmentedInformation() +
factor3->augmentedInformation();
// Check Information vector // Check Information vector
Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector Vector InfoVector = AugInformationMatrix.block(
0, 18, 18, 1); // 18x18 Hessian + information vector
// Check Hessian // Check Hessian
EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
@ -814,7 +827,6 @@ TEST( SmartProjectionRigFactor, CheckHessian) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, Hessian) { TEST(SmartProjectionRigFactor, Hessian) {
using namespace vanillaPose2; using namespace vanillaPose2;
KeyVector views{x1, x2}; KeyVector views{x1, x2};
@ -833,8 +845,8 @@ TEST( SmartProjectionRigFactor, Hessian ) {
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig));
smartFactor1->add(measurements_cam1, views, cameraIds); smartFactor1->add(measurements_cam1, views, cameraIds);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Pose3 noise_pose =
Point3(0.5, 0.1, 0.3)); Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3));
Values values; Values values;
values.insert(x1, cam1.pose()); values.insert(x1, cam1.pose());
values.insert(x2, cam2.pose()); values.insert(x2, cam2.pose());
@ -843,8 +855,9 @@ TEST( SmartProjectionRigFactor, Hessian ) {
// compute triangulation from linearization point // compute triangulation from linearization point
// compute reprojection errors (sum squared) // compute reprojection errors (sum squared)
// compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) // compare with factor.info(): the bottom right element is the squared sum of
// check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] // the reprojection errors (normalized by the covariance) check that it is
// correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -861,7 +874,6 @@ TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, Cal3Bundler) { TEST(SmartProjectionRigFactor, Cal3Bundler) {
using namespace bundlerPose; using namespace bundlerPose;
// three landmarks ~5 meters in front of camera // three landmarks ~5 meters in front of camera
@ -898,18 +910,18 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) {
graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.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/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), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, cam1.pose()); values.insert(x1, cam1.pose());
values.insert(x2, cam2.pose()); values.insert(x2, cam2.pose());
// initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
EXPECT( EXPECT(assert_equal(
assert_equal( Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
Pose3(
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), Point3(0.1, -0.1, 1.9)),
values.at<Pose3>(x3))); values.at<Pose3>(x3)));
@ -924,9 +936,11 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) {
typedef GenericProjectionFactor<Pose3, Point3> TestProjectionFactor; typedef GenericProjectionFactor<Pose3, Point3> TestProjectionFactor;
static Symbol l0('L', 0); static Symbol l0('L', 0);
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { TEST(SmartProjectionRigFactor,
// in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark hessianComparedToProjFactors_measurementsFromSamePose) {
// at a single pose, a setup that occurs in multi-camera systems // 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; using namespace vanillaPose;
Point2Vector measurements_lmk1; Point2Vector measurements_lmk1;
@ -936,10 +950,11 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
// create redundant measurements: // create redundant measurements:
Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1;
measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement measurements_lmk1_redundant.push_back(
measurements_lmk1.at(0)); // we readd the first measurement
// create inputs // create inputs
std::vector<Key> keys { x1, x2, x3, x1}; KeyVector keys{x1, x2, x3, x1};
Cameras cameraRig; // single camera in the rig Cameras cameraRig; // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK));
@ -953,10 +968,15 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
// initialize third pose with some noise to get a nontrivial linearization point // initialize third pose with some noise to get a nontrivial linearization
// point
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy 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))); 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 // linearization point for the poses
Pose3 pose1 = level_pose; Pose3 pose1 = level_pose;
@ -965,8 +985,8 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
// ==== check Hessian of smartFactor1 ===== // ==== check Hessian of smartFactor1 =====
// -- compute actual Hessian // -- compute actual Hessian
boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize( boost::shared_ptr<GaussianFactor> linearfactor1 =
values); smartFactor1->linearize(values);
Matrix actualHessian = linearfactor1->information(); Matrix actualHessian = linearfactor1->information();
// -- compute expected Hessian from manual Schur complement from Jacobians // -- compute expected Hessian from manual Schur complement from Jacobians
@ -984,30 +1004,31 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0,
sharedK); sharedK);
Matrix HPoseActual, HEActual; Matrix HPoseActual, HEActual;
// note: b is minus the reprojection error, cf the smart factor jacobian computation // note: b is minus the reprojection error, cf the smart factor jacobian
b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, // computation
HEActual); b.segment<2>(0) =
-factor11.evaluateError(pose1, *point, HPoseActual, HEActual);
F.block<2, 6>(0, 0) = HPoseActual; F.block<2, 6>(0, 0) = HPoseActual;
E.block<2, 3>(0, 0) = HEActual; E.block<2, 3>(0, 0) = HEActual;
TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0,
sharedK); sharedK);
b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, b.segment<2>(2) =
HEActual); -factor12.evaluateError(pose2, *point, HPoseActual, HEActual);
F.block<2, 6>(2, 6) = HPoseActual; F.block<2, 6>(2, 6) = HPoseActual;
E.block<2, 3>(2, 0) = HEActual; E.block<2, 3>(2, 0) = HEActual;
TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0,
sharedK); sharedK);
b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, b.segment<2>(4) =
HEActual); -factor13.evaluateError(pose3, *point, HPoseActual, HEActual);
F.block<2, 6>(4, 12) = HPoseActual; F.block<2, 6>(4, 12) = HPoseActual;
E.block<2, 3>(4, 0) = HEActual; E.block<2, 3>(4, 0) = HEActual;
TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0,
sharedK); sharedK);
b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, b.segment<2>(6) =
HEActual); -factor11.evaluateError(pose1, *point, HPoseActual, HEActual);
F.block<2, 6>(6, 0) = HPoseActual; F.block<2, 6>(6, 0) = HPoseActual;
E.block<2, 3>(6, 0) = HEActual; E.block<2, 3>(6, 0) = HEActual;
@ -1017,20 +1038,22 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
b = (1 / sigma) * b; b = (1 / sigma) * b;
//* G = F' * F - F' * E * P * E' * F //* G = F' * F - F' * E * P * E' * F
Matrix P = (E.transpose() * E).inverse(); Matrix P = (E.transpose() * E).inverse();
Matrix expectedHessian = F.transpose() * F Matrix expectedHessian =
- (F.transpose() * E * P * E.transpose() * F); F.transpose() * F - (F.transpose() * E * P * E.transpose() * F);
EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6));
// ==== check Information vector of smartFactor1 ===== // ==== check Information vector of smartFactor1 =====
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
gfg.add(linearfactor1); gfg.add(linearfactor1);
Matrix actualHessian_v2 = gfg.hessian().first; Matrix actualHessian_v2 = gfg.hessian().first;
EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian EXPECT(assert_equal(actualHessian_v2, actualHessian,
1e-6)); // sanity check on hessian
// -- compute actual information vector // -- compute actual information vector
Vector actualInfoVector = gfg.hessian().second; Vector actualInfoVector = gfg.hessian().second;
// -- compute expected information vector from manual Schur complement from Jacobians // -- compute expected information vector from manual Schur complement from
// Jacobians
//* g = F' * (b - E * P * E' * b) //* g = F' * (b - E * P * E' * b)
Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b);
EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6));
@ -1050,7 +1073,6 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
using namespace vanillaPose; using namespace vanillaPose;
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
@ -1060,21 +1082,24 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
// create inputs // create inputs
std::vector<Key> keys { x1, x2, x3 }; KeyVector keys{x1, x2, x3};
Cameras cameraRig; // single camera in the rig Cameras cameraRig; // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
FastVector<size_t> cameraIdsRedundant{0, 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 // For first factor, we create redundant measurement (taken by the same keys
// make sure the redundancy in the keys does not create problems) // as factor 1, to make sure the redundancy in the keys does not create
// problems)
Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1;
measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement measurements_lmk1_redundant.push_back(
std::vector<Key> keys_redundant = keys; measurements_lmk1.at(0)); // we readd the first measurement
KeyVector keys_redundant = keys;
keys_redundant.push_back(keys.at(0)); // we readd the first key keys_redundant.push_back(keys.at(0)); // we readd the first key
SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig));
smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant); smartFactor1->add(measurements_lmk1_redundant, keys_redundant,
cameraIdsRedundant);
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig));
smartFactor2->add(measurements_lmk2, keys, cameraIds); smartFactor2->add(measurements_lmk2, keys, cameraIds);
@ -1097,20 +1122,22 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) {
groundTruth.insert(x3, pose_above); groundTruth.insert(x3, pose_above);
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); 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/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), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, level_pose);
values.insert(x2, pose_right); values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to original pose_above // initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose); values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy EXPECT( // check that the pose is actually noisy
assert_equal( assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
Pose3( -0.0313952598, -0.000986635786, 0.0314107591,
Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.999013364, -0.0313952598),
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)),
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3))); values.at<Pose3>(x3)));
Values result; Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
@ -1120,13 +1147,14 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) {
#ifndef DISABLE_TIMING #ifndef DISABLE_TIMING
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
// this factor is slightly slower (but comparable) to original SmartProjectionPoseFactor // this factor is slightly slower (but comparable) to original
// SmartProjectionPoseFactor
//-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) //-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) //| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11
//| -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 times, 0.065103 wall, 0.06 children, min: 0 max: 0) // 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) { TEST(SmartProjectionRigFactor, timing) {
using namespace vanillaPose; using namespace vanillaPose;
// Default cameras for simple derivatives // Default cameras for simple derivatives
@ -1153,7 +1181,8 @@ TEST( SmartProjectionRigFactor, timing ) {
size_t nrTests = 10000; size_t nrTests = 10000;
for (size_t i = 0; i < nrTests; i++) { for (size_t i = 0; i < nrTests; i++) {
SmartRigFactor::shared_ptr smartFactorP(new SmartRigFactor(model, cameraRig)); SmartRigFactor::shared_ptr smartFactorP(
new SmartRigFactor(model, cameraRig));
smartFactorP->add(measurements_lmk1[0], x1, cameraId1); smartFactorP->add(measurements_lmk1[0], x1, cameraId1);
smartFactorP->add(measurements_lmk1[1], x1, cameraId1); smartFactorP->add(measurements_lmk1[1], x1, cameraId1);
@ -1181,12 +1210,17 @@ TEST( SmartProjectionRigFactor, timing ) {
} }
#endif #endif
///* ************************************************************************* */ ///* *************************************************************************
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); ///*/
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); // BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); // "gtsam_noiseModel_Constrained");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
// "gtsam_noiseModel_Diagonal");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
// "gtsam_noiseModel_Gaussian");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); // BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); // BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
// "gtsam_noiseModel_Isotropic");
// BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); // BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
// BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); // BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
// //
@ -1235,4 +1269,3 @@ int main() {
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */

81
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -84,7 +84,8 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -98,7 +99,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, add) { TEST(SmartProjectionPoseFactorRollingShutter, add) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK)));
factor1->add(measurement1, x1, x2, interp_factor); factor1->add(measurement1, x1, x2, interp_factor);
} }
@ -122,7 +124,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
std::vector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
Cameras cameraRig; Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensor, sharedK)); cameraRig.push_back(Camera(body_P_sensor, sharedK));
@ -164,7 +166,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
// returns false (use default cameraIds) // returns false (use default cameraIds)
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig));
factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! factor1->add(measurement2, x2, x2, interp_factor2,
cameraId1); // different!
factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor2));
@ -176,7 +179,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK));
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2));
factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); // different! factor1->add(measurement2, x2, x3, interp_factor2,
cameraId1); // different!
factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor2));
@ -186,7 +190,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
// equal returns false // equal returns false
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig));
factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement2, x2, x3, interp_factor1, cameraId1); // different! factor1->add(measurement2, x2, x3, interp_factor1,
cameraId1); // different!
factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor2));
@ -377,13 +382,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK)));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK)));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); SmartFactorRS::shared_ptr smartFactor3(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK)));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -503,20 +511,21 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) {
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
// create arbitrary body_T_sensor (transforms from sensor to body) // create arbitrary body_T_sensor (transforms from sensor to body)
Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-0.03, 0., 0.01), Point3(1, 1, 1));
Point3(1, 1, 1)); Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-0.1, 0., 0.05), Point3(0, 0, 1));
Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Pose3 body_T_sensor3 = Pose3(Rot3::Ypr(-0.3, 0., -0.05), Point3(0, 1, 1));
Point3(0, 0, 1));
Pose3 body_T_sensor3 = Pose3::identity();
Camera camera1(interp_pose1 * body_T_sensor1, sharedK); Camera camera1(interp_pose1 * body_T_sensor1, sharedK);
Camera camera2(interp_pose2 * body_T_sensor2, sharedK); Camera camera2(interp_pose2 * body_T_sensor2, sharedK);
Camera camera3(interp_pose3 * body_T_sensor3, sharedK); Camera camera3(interp_pose3 * body_T_sensor3, sharedK);
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(camera1, camera2, camera3, landmark1, measurements_lmk1); projectToMultipleCameras(camera1, camera2, camera3, landmark1,
projectToMultipleCameras(camera1, camera2, camera3, landmark2, measurements_lmk2); measurements_lmk1);
projectToMultipleCameras(camera1, camera2, camera3, landmark3, measurements_lmk3); projectToMultipleCameras(camera1, camera2, camera3, landmark2,
measurements_lmk2);
projectToMultipleCameras(camera1, camera2, camera3, landmark3,
measurements_lmk3);
// create inputs // create inputs
std::vector<std::pair<Key, Key>> key_pairs; std::vector<std::pair<Key, Key>> key_pairs;
@ -608,7 +617,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
measurements_lmk1.push_back(cam1.project(landmark1)); measurements_lmk1.push_back(cam1.project(landmark1));
measurements_lmk1.push_back(cam2.project(landmark1)); measurements_lmk1.push_back(cam2.project(landmark1));
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple)));
double interp_factor = 0; // equivalent to measurement taken at pose 1 double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor);
interp_factor = 1; // equivalent to measurement taken at pose 2 interp_factor = 1; // equivalent to measurement taken at pose 2
@ -841,16 +851,20 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
params.setEnableEPI(false); params.setEnableEPI(false);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); SmartFactorRS::shared_ptr smartFactor3(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); SmartFactorRS::shared_ptr smartFactor4(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor4->add(measurements_lmk4, key_pairs, interp_factors); smartFactor4->add(measurements_lmk4, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -901,7 +915,8 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK)));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -1038,7 +1053,8 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK)));
smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -1193,14 +1209,17 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors_redundant.push_back( interp_factors_redundant.push_back(
interp_factors.at(0)); // we readd the first interp factor interp_factors.at(0)); // we readd the first interp factor
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK)));
smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
interp_factors_redundant); interp_factors_redundant);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK)));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); SmartFactorRS::shared_ptr smartFactor3(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK)));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1244,8 +1263,9 @@ TEST(SmartProjectionPoseFactorRollingShutter,
#ifndef DISABLE_TIMING #ifndef DISABLE_TIMING
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
//-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0) //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0)
//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, min: 0 max: 0) //| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children,
//| -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 children, min: 0 max: 0) // min: 0 max: 0) | -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06
// children, min: 0 max: 0)
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, timing) { TEST(SmartProjectionPoseFactorRollingShutter, timing) {
using namespace vanillaPose; using namespace vanillaPose;
@ -1271,7 +1291,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
size_t nrTests = 10000; size_t nrTests = 10000;
for (size_t i = 0; i < nrTests; i++) { for (size_t i = 0; i < nrTests; i++) {
SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); SmartFactorRS::shared_ptr smartFactorRS(
new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple)));
double interp_factor = 0; // equivalent to measurement taken at pose 1 double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
interp_factor = 1; // equivalent to measurement taken at pose 2 interp_factor = 1; // equivalent to measurement taken at pose 2