fixed formatting (plus small fix: std::vector -> fastVector)
lcarlone
parent
737dcf65e4
commit
c4cd2b5080
|
|
@ -19,15 +19,17 @@
|
|||
* @date August 2021
|
||||
*/
|
||||
|
||||
#include "smartFactorScenarios.h"
|
||||
#include <gtsam/slam/PoseTranslationPrior.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/numericalDerivative.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 <iostream>
|
||||
|
||||
#include "smartFactorScenarios.h"
|
||||
|
||||
using namespace boost::assign;
|
||||
using namespace std::placeholders;
|
||||
|
||||
|
|
@ -37,8 +39,8 @@ static const double sigma = 0.1;
|
|||
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
|
||||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::L;
|
||||
using symbol_shorthand::X;
|
||||
|
||||
// tests data
|
||||
static Symbol x1('X', 1);
|
||||
|
|
@ -115,7 +117,8 @@ TEST( SmartProjectionRigFactor, Equals ) {
|
|||
|
||||
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
|
||||
|
||||
CHECK(assert_equal(*factor1, *factor3));
|
||||
|
|
@ -123,7 +126,6 @@ TEST( SmartProjectionRigFactor, Equals ) {
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionRigFactor, noiseless) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
// Project two landmarks into two cameras
|
||||
|
|
@ -182,7 +184,6 @@ TEST( SmartProjectionRigFactor, noiseless ) {
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionRigFactor, noisy) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
|
|
@ -195,8 +196,8 @@ TEST( SmartProjectionRigFactor, noisy ) {
|
|||
|
||||
Values values;
|
||||
values.insert(x1, cam1.pose());
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
|
||||
Point3(0.5, 0.1, 0.3));
|
||||
Pose3 noise_pose =
|
||||
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));
|
||||
|
||||
SmartRigFactor::shared_ptr factor(new SmartRigFactor(model, cameraRig));
|
||||
|
|
@ -225,8 +226,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
|
|||
using namespace vanillaPose;
|
||||
|
||||
// create arbitrary body_T_sensor (transforms from sensor to body)
|
||||
Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
|
||||
Point3(1, 1, 1));
|
||||
Pose3 body_T_sensor =
|
||||
Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(body_T_sensor, sharedK));
|
||||
|
||||
|
|
@ -256,7 +257,9 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
|
|||
params.setEnableEPI(false);
|
||||
|
||||
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);
|
||||
smartFactor2.add(measurements_cam2, views);
|
||||
|
|
@ -283,8 +286,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
|
|||
double expectedError = 0.0;
|
||||
DOUBLES_EQUAL(expectedError, actualError, 1e-7)
|
||||
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
Point3(0.1, 0.1, 0.1));
|
||||
Pose3 noise_pose =
|
||||
Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1));
|
||||
Values values;
|
||||
values.insert(x1, wTb1);
|
||||
values.insert(x2, wTb2);
|
||||
|
|
@ -304,10 +307,10 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
|
|||
using namespace vanillaPose;
|
||||
|
||||
// create arbitrary body_T_sensor (transforms from sensor to body)
|
||||
Pose3 body_T_sensor1 = 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),
|
||||
Point3(0, 0, 1));
|
||||
Pose3 body_T_sensor1 =
|
||||
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), Point3(0, 0, 1));
|
||||
Pose3 body_T_sensor3 = Pose3::identity();
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
|
|
@ -370,8 +373,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
|
|||
double expectedError = 0.0;
|
||||
DOUBLES_EQUAL(expectedError, actualError, 1e-7)
|
||||
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
Point3(0.1, 0.1, 0.1));
|
||||
Pose3 noise_pose =
|
||||
Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1));
|
||||
Values values;
|
||||
values.insert(x1, wTb1);
|
||||
values.insert(x2, wTb2);
|
||||
|
|
@ -388,7 +391,6 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
|
||||
|
||||
using namespace vanillaPose2;
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
|
|
@ -401,7 +403,8 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
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));
|
||||
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||
|
|
@ -427,18 +430,18 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) {
|
|||
groundTruth.insert(x3, cam3.pose());
|
||||
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),
|
||||
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
|
||||
// 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,
|
||||
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)));
|
||||
|
|
@ -451,14 +454,13 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) {
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionRigFactor, Factors) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
// Default cameras for simple derivatives
|
||||
Rot3 R;
|
||||
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0));
|
||||
Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2(
|
||||
Pose3(R, Point3(1, 0, 0)), sharedK);
|
||||
Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK),
|
||||
cam2(Pose3(R, Point3(1, 0, 0)), sharedK);
|
||||
|
||||
// one landmarks 1m in front of camera
|
||||
Point3 landmark1(0, 0, 10);
|
||||
|
|
@ -473,9 +475,10 @@ TEST( SmartProjectionRigFactor, Factors ) {
|
|||
KeyVector views{x1, x2};
|
||||
FastVector<size_t> cameraIds{0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor
|
||||
> (model, Camera(Pose3::identity(), sharedK));
|
||||
smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds
|
||||
SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
|
||||
model, Camera(Pose3::identity(), sharedK));
|
||||
smartFactor1->add(measurements_cam1,
|
||||
views); // we have a single camera so use default cameraIds
|
||||
|
||||
SmartRigFactor::Cameras cameras;
|
||||
cameras.push_back(cam1);
|
||||
|
|
@ -501,7 +504,8 @@ TEST( SmartProjectionRigFactor, Factors ) {
|
|||
perturbedDelta.insert(x2, delta);
|
||||
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;
|
||||
A1 << -10, 0, 0, 0, 1, 0;
|
||||
A2 << 10, 0, 1, 0, -1, 0;
|
||||
|
|
@ -528,8 +532,8 @@ TEST( SmartProjectionRigFactor, Factors ) {
|
|||
values.insert(x1, cam1.pose());
|
||||
values.insert(x2, cam2.pose());
|
||||
|
||||
boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1
|
||||
->createHessianFactor(values, 0.0);
|
||||
boost::shared_ptr<RegularHessianFactor<6>> actual =
|
||||
smartFactor1->createHessianFactor(values, 0.0);
|
||||
EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6));
|
||||
EXPECT(assert_equal(expected, *actual, 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) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
KeyVector views{x1, x2, x3};
|
||||
|
|
@ -578,20 +581,19 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) {
|
|||
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/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
|
||||
// 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(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656,
|
||||
-0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
|
||||
-0.0313952598),
|
||||
EXPECT(assert_equal(Pose3(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)),
|
||||
values.at<Pose3>(x3)));
|
||||
|
||||
|
|
@ -603,7 +605,6 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) {
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionRigFactor, landmarkDistance) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
double excludeLandmarksFutherThanDist = 2;
|
||||
|
|
@ -628,13 +629,16 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) {
|
|||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
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);
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params));
|
||||
SmartRigFactor::shared_ptr smartFactor2(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
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);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -646,7 +650,8 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) {
|
|||
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/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;
|
||||
|
|
@ -663,11 +668,11 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) {
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
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};
|
||||
|
||||
|
|
@ -682,7 +687,8 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
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;
|
||||
params.setLinearizationMode(gtsam::HESSIAN);
|
||||
|
|
@ -694,16 +700,20 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
|
|||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
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);
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params));
|
||||
SmartRigFactor::shared_ptr smartFactor2(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
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);
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor4(new SmartRigFactor(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);
|
||||
|
|
@ -730,14 +740,13 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionRigFactor, CheckHessian) {
|
||||
|
||||
KeyVector views{x1, x2, x3};
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
// Two slightly different cameras
|
||||
Pose3 pose2 = level_pose
|
||||
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
|
||||
Pose3 pose2 =
|
||||
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));
|
||||
Camera cam2(pose2, sharedK);
|
||||
Camera cam3(pose3, sharedK);
|
||||
|
|
@ -757,13 +766,16 @@ TEST( SmartProjectionRigFactor, CheckHessian) {
|
|||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
|
|
@ -771,20 +783,19 @@ TEST( SmartProjectionRigFactor, CheckHessian) {
|
|||
graph.push_back(smartFactor2);
|
||||
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),
|
||||
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
|
||||
// initialize third pose with some noise, we expect it to move back to
|
||||
// original pose_above
|
||||
values.insert(x3, pose3 * noise_pose);
|
||||
EXPECT(
|
||||
assert_equal(
|
||||
Pose3(
|
||||
Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
|
||||
-0.130426831, -0.0115837907, 0.130819108, -0.98278564,
|
||||
-0.130455917),
|
||||
EXPECT(assert_equal(Pose3(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)),
|
||||
values.at<Pose3>(x3)));
|
||||
|
||||
|
|
@ -792,21 +803,23 @@ TEST( SmartProjectionRigFactor, CheckHessian) {
|
|||
boost::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
|
||||
boost::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values);
|
||||
|
||||
Matrix CumulativeInformation = factor1->information() + factor2->information()
|
||||
+ factor3->information();
|
||||
Matrix CumulativeInformation =
|
||||
factor1->information() + factor2->information() + factor3->information();
|
||||
|
||||
boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(
|
||||
values);
|
||||
boost::shared_ptr<GaussianFactorGraph> GaussianGraph =
|
||||
graph.linearize(values);
|
||||
Matrix GraphInformation = GaussianGraph->hessian().first;
|
||||
|
||||
// Check Hessian
|
||||
EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));
|
||||
|
||||
Matrix AugInformationMatrix = factor1->augmentedInformation()
|
||||
+ factor2->augmentedInformation() + factor3->augmentedInformation();
|
||||
Matrix AugInformationMatrix = factor1->augmentedInformation() +
|
||||
factor2->augmentedInformation() +
|
||||
factor3->augmentedInformation();
|
||||
|
||||
// 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
|
||||
EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
|
||||
|
|
@ -814,7 +827,6 @@ TEST( SmartProjectionRigFactor, CheckHessian) {
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionRigFactor, Hessian) {
|
||||
|
||||
using namespace vanillaPose2;
|
||||
|
||||
KeyVector views{x1, x2};
|
||||
|
|
@ -833,8 +845,8 @@ TEST( SmartProjectionRigFactor, Hessian ) {
|
|||
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),
|
||||
Point3(0.5, 0.1, 0.3));
|
||||
Pose3 noise_pose =
|
||||
Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3));
|
||||
Values values;
|
||||
values.insert(x1, cam1.pose());
|
||||
values.insert(x2, cam2.pose());
|
||||
|
|
@ -843,8 +855,9 @@ TEST( SmartProjectionRigFactor, Hessian ) {
|
|||
|
||||
// compute triangulation from linearization point
|
||||
// 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)
|
||||
// check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
|
||||
// compare with factor.info(): the bottom right element is the squared sum of
|
||||
// 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) {
|
||||
|
||||
using namespace bundlerPose;
|
||||
|
||||
// three landmarks ~5 meters in front of camera
|
||||
|
|
@ -898,18 +910,18 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) {
|
|||
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/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
|
||||
// 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,
|
||||
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)));
|
||||
|
|
@ -924,9 +936,11 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) {
|
|||
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
|
||||
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;
|
||||
|
|
@ -936,10 +950,11 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
|
|||
|
||||
// create redundant measurements:
|
||||
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
|
||||
std::vector<Key> keys { x1, x2, x3, x1};
|
||||
KeyVector keys{x1, x2, x3, x1};
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
|
|
@ -953,10 +968,15 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
|
|||
Values values;
|
||||
values.insert(x1, level_pose);
|
||||
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);
|
||||
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
|
||||
Pose3 pose1 = level_pose;
|
||||
|
|
@ -965,8 +985,8 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
|
|||
|
||||
// ==== check Hessian of smartFactor1 =====
|
||||
// -- compute actual Hessian
|
||||
boost::shared_ptr<GaussianFactor> linearfactor1 = smartFactor1->linearize(
|
||||
values);
|
||||
boost::shared_ptr<GaussianFactor> linearfactor1 =
|
||||
smartFactor1->linearize(values);
|
||||
Matrix actualHessian = linearfactor1->information();
|
||||
|
||||
// -- 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,
|
||||
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);
|
||||
// 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);
|
||||
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);
|
||||
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);
|
||||
b.segment<2>(6) =
|
||||
-factor11.evaluateError(pose1, *point, HPoseActual, HEActual);
|
||||
F.block<2, 6>(6, 0) = HPoseActual;
|
||||
E.block<2, 3>(6, 0) = HEActual;
|
||||
|
||||
|
|
@ -1017,20 +1038,22 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
|
|||
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);
|
||||
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
|
||||
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
|
||||
// -- 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));
|
||||
|
|
@ -1050,7 +1073,6 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam
|
|||
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
|
||||
|
||||
|
|
@ -1060,21 +1082,24 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
|
||||
|
||||
// create inputs
|
||||
std::vector<Key> keys { x1, x2, x3 };
|
||||
KeyVector 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)
|
||||
// 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;
|
||||
measurements_lmk1_redundant.push_back(
|
||||
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
|
||||
|
||||
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));
|
||||
smartFactor2->add(measurements_lmk2, keys, cameraIds);
|
||||
|
|
@ -1097,20 +1122,22 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) {
|
|||
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/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
|
||||
// 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)));
|
||||
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);
|
||||
|
|
@ -1120,13 +1147,14 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) {
|
|||
|
||||
#ifndef DISABLE_TIMING
|
||||
#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)
|
||||
//| -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)
|
||||
//| -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
|
||||
|
|
@ -1153,7 +1181,8 @@ TEST( SmartProjectionRigFactor, timing ) {
|
|||
size_t nrTests = 10000;
|
||||
|
||||
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[1], x1, cameraId1);
|
||||
|
||||
|
|
@ -1181,12 +1210,17 @@ TEST( SmartProjectionRigFactor, timing ) {
|
|||
}
|
||||
#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::Gaussian, "gtsam_noiseModel_Gaussian");
|
||||
///* *************************************************************************
|
||||
///*/
|
||||
// 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::Gaussian,
|
||||
// "gtsam_noiseModel_Gaussian");
|
||||
// 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::SharedDiagonal, "gtsam_SharedDiagonal");
|
||||
//
|
||||
|
|
@ -1235,4 +1269,3 @@ int main() {
|
|||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -84,7 +84,8 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
|
|||
/* ************************************************************************* */
|
||||
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
|
||||
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) {
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -122,7 +124,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
|
|||
interp_factors.push_back(interp_factor2);
|
||||
interp_factors.push_back(interp_factor3);
|
||||
|
||||
std::vector<size_t> cameraIds{0, 0, 0};
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
|
||||
Cameras cameraRig;
|
||||
cameraRig.push_back(Camera(body_P_sensor, sharedK));
|
||||
|
|
@ -164,7 +166,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
|
|||
// returns false (use default cameraIds)
|
||||
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig));
|
||||
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);
|
||||
|
||||
EXPECT(!factor1->equals(*factor2));
|
||||
|
|
@ -176,7 +179,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
|
|||
cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK));
|
||||
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2));
|
||||
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);
|
||||
|
||||
EXPECT(!factor1->equals(*factor2));
|
||||
|
|
@ -186,7 +190,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
|
|||
// equal returns false
|
||||
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig));
|
||||
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);
|
||||
|
||||
EXPECT(!factor1->equals(*factor2));
|
||||
|
|
@ -377,13 +382,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
|
|||
interp_factors.push_back(interp_factor2);
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
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;
|
||||
|
||||
// create arbitrary body_T_sensor (transforms from sensor to body)
|
||||
Pose3 body_T_sensor1 = 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),
|
||||
Point3(0, 0, 1));
|
||||
Pose3 body_T_sensor3 = Pose3::identity();
|
||||
Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-0.03, 0., 0.01), Point3(1, 1, 1));
|
||||
Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-0.1, 0., 0.05), Point3(0, 0, 1));
|
||||
Pose3 body_T_sensor3 = Pose3(Rot3::Ypr(-0.3, 0., -0.05), Point3(0, 1, 1));
|
||||
|
||||
Camera camera1(interp_pose1 * body_T_sensor1, sharedK);
|
||||
Camera camera2(interp_pose2 * body_T_sensor2, sharedK);
|
||||
Camera camera3(interp_pose3 * body_T_sensor3, sharedK);
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(camera1, camera2, camera3, landmark1, measurements_lmk1);
|
||||
projectToMultipleCameras(camera1, camera2, camera3, landmark2, measurements_lmk2);
|
||||
projectToMultipleCameras(camera1, camera2, camera3, landmark3, measurements_lmk3);
|
||||
projectToMultipleCameras(camera1, camera2, camera3, landmark1,
|
||||
measurements_lmk1);
|
||||
projectToMultipleCameras(camera1, camera2, camera3, landmark2,
|
||||
measurements_lmk2);
|
||||
projectToMultipleCameras(camera1, camera2, camera3, landmark3,
|
||||
measurements_lmk3);
|
||||
|
||||
// create inputs
|
||||
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(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
|
||||
smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor);
|
||||
interp_factor = 1; // equivalent to measurement taken at pose 2
|
||||
|
|
@ -841,16 +851,20 @@ TEST(SmartProjectionPoseFactorRollingShutter,
|
|||
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
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_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);
|
||||
|
||||
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_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);
|
||||
|
||||
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.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,
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -1244,8 +1263,9 @@ TEST(SmartProjectionPoseFactorRollingShutter,
|
|||
#ifndef DISABLE_TIMING
|
||||
#include <gtsam/base/timing.h>
|
||||
//-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)
|
||||
//| -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 children, min: 0 max: 0)
|
||||
//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children,
|
||||
// min: 0 max: 0) | -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06
|
||||
// children, min: 0 max: 0)
|
||||
/* *************************************************************************/
|
||||
TEST(SmartProjectionPoseFactorRollingShutter, timing) {
|
||||
using namespace vanillaPose;
|
||||
|
|
@ -1271,7 +1291,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
|
|||
size_t nrTests = 10000;
|
||||
|
||||
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
|
||||
smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
|
||||
interp_factor = 1; // equivalent to measurement taken at pose 2
|
||||
|
|
|
|||
Loading…
Reference in New Issue