Moved scenarios to separate header
dellaert
parent
a60e13dd09
commit
cd10f9aedc
|
|
@ -0,0 +1,120 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SmartFactorScenarios.h
|
||||
* @brief Scenarios for testSmart*.cpp
|
||||
* @author Frank Dellaert
|
||||
* @date Feb 2015
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam/slam/SmartProjectionCameraFactor.h>
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
Point3 landmark4(10, 0.5, 1.2);
|
||||
Point3 landmark5(10, -0.5, 1.2);
|
||||
|
||||
// First camera pose, looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
|
||||
// Second camera 1 meter to the right of first camera
|
||||
Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
|
||||
// Third camera 1 meter above the first camera
|
||||
Pose3 pose_up = level_pose * Pose3(Rot3(), Point3(0, -1, 0));
|
||||
|
||||
// Create a noise unit2 for the pixel error
|
||||
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
|
||||
|
||||
/* ************************************************************************* */
|
||||
// default Cal3_S2 cameras
|
||||
namespace vanilla {
|
||||
typedef PinholeCamera<Cal3_S2> Camera;
|
||||
// make a realistic calibration matrix
|
||||
static double fov = 60; // degrees
|
||||
static size_t w = 640, h = 480;
|
||||
static Cal3_S2 K(fov, w, h);
|
||||
static Cal3_S2 K2(1500, 1200, 0, w, h);
|
||||
Camera level_camera(level_pose, K2);
|
||||
Camera level_camera_right(pose_right, K2);
|
||||
Point2 level_uv = level_camera.project(landmark1);
|
||||
Point2 level_uv_right = level_camera_right.project(landmark1);
|
||||
Camera cam1(level_pose, K2);
|
||||
Camera cam2(pose_right, K2);
|
||||
Camera cam3(pose_up, K2);
|
||||
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
||||
typedef SmartProjectionCameraFactor<Cal3_S2> SmartFactor;
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
// Cal3Bundler cameras
|
||||
namespace bundler {
|
||||
typedef PinholeCamera<Cal3Bundler> Camera;
|
||||
static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
|
||||
Camera level_camera(level_pose, K);
|
||||
Camera level_camera_right(pose_right, K);
|
||||
Point2 level_uv = level_camera.project(landmark1);
|
||||
Point2 level_uv_right = level_camera_right.project(landmark1);
|
||||
Pose3 pose1 = level_pose;
|
||||
Camera cam1(level_pose, K);
|
||||
Camera cam2(pose_right, K);
|
||||
Camera cam3(pose_up, K);
|
||||
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
||||
typedef SmartProjectionCameraFactor<Cal3Bundler> SmartFactorBundler;
|
||||
}
|
||||
/* *************************************************************************/
|
||||
|
||||
template<class CALIBRATION>
|
||||
PinholeCamera<CALIBRATION> perturbCameraPose(
|
||||
const PinholeCamera<CALIBRATION>& camera) {
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
|
||||
Point3(0.5, 0.1, 0.3));
|
||||
Pose3 cameraPose = camera.pose();
|
||||
Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
|
||||
return PinholeCamera<CALIBRATION>(perturbedCameraPose, camera.calibration());
|
||||
}
|
||||
|
||||
template<class CALIBRATION>
|
||||
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
|
||||
const PinholeCamera<CALIBRATION>& camera) {
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
|
||||
Point3(0.5, 0.1, 0.3));
|
||||
Pose3 cameraPose = camera.pose();
|
||||
Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
|
||||
typename gtsam::traits<CALIBRATION>::TangentVector d;
|
||||
d.setRandom();
|
||||
d *= 0.1;
|
||||
CALIBRATION perturbedCalibration = camera.calibration().retract(d);
|
||||
return PinholeCamera<CALIBRATION>(perturbedCameraPose, perturbedCalibration);
|
||||
}
|
||||
|
||||
template<class CALIBRATION>
|
||||
void projectToMultipleCameras(const PinholeCamera<CALIBRATION>& cam1,
|
||||
const PinholeCamera<CALIBRATION>& cam2,
|
||||
const PinholeCamera<CALIBRATION>& cam3, Point3 landmark,
|
||||
vector<Point2>& measurements_cam) {
|
||||
Point2 cam1_uv1 = cam1.project(landmark);
|
||||
Point2 cam2_uv1 = cam2.project(landmark);
|
||||
Point2 cam3_uv1 = cam3.project(landmark);
|
||||
measurements_cam.push_back(cam1_uv1);
|
||||
measurements_cam.push_back(cam2_uv1);
|
||||
measurements_cam.push_back(cam3_uv1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -18,36 +18,19 @@
|
|||
* @date Sept 2013
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/SmartProjectionCameraFactor.h>
|
||||
|
||||
#include "smartFactorScenarios.h"
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/slam/PoseTranslationPrior.h>
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
|
||||
//#include "../SmartNonlinearFactorGraph.h"
|
||||
#undef CHECK
|
||||
#include <boost/assign/std/map.hpp>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
//
|
||||
using namespace boost::assign;
|
||||
using namespace gtsam;
|
||||
|
||||
static bool isDebugTest = false;
|
||||
|
||||
// make a realistic calibration matrix
|
||||
static double fov = 60; // degrees
|
||||
static size_t w = 640, h = 480;
|
||||
|
||||
static double rankTol = 1.0;
|
||||
static double linThreshold = -1.0;
|
||||
|
||||
// Create a noise model for the pixel error
|
||||
static SharedNoiseModel model(noiseModel::Unit::Create(2));
|
||||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::L;
|
||||
|
|
@ -56,70 +39,10 @@ static Key x1(1);
|
|||
Symbol l1('l', 1), l2('l', 2), l3('l', 3);
|
||||
Key c1 = 1, c2 = 2, c3 = 3;
|
||||
|
||||
// First camera pose, looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
|
||||
// Second camera 1 meter to the right of first camera
|
||||
Pose3 pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
|
||||
// Third camera 1 meter above the first camera
|
||||
Pose3 pose_up = level_pose * Pose3(Rot3(), Point3(0, -1, 0));
|
||||
|
||||
static Point2 measurement1(323.0, 240.0);
|
||||
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
|
||||
Point3(0.25, -0.10, 1.0));
|
||||
|
||||
template<class CALIBRATION>
|
||||
PinholeCamera<CALIBRATION> perturbCameraPose(
|
||||
const PinholeCamera<CALIBRATION>& camera) {
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
|
||||
Point3(0.5, 0.1, 0.3));
|
||||
Pose3 cameraPose = camera.pose();
|
||||
Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
|
||||
return PinholeCamera<CALIBRATION>(perturbedCameraPose, camera.calibration());
|
||||
}
|
||||
|
||||
template<class CALIBRATION>
|
||||
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
|
||||
const PinholeCamera<CALIBRATION>& camera) {
|
||||
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
|
||||
Point3(0.5, 0.1, 0.3));
|
||||
Pose3 cameraPose = camera.pose();
|
||||
Pose3 perturbedCameraPose = cameraPose.compose(noise_pose);
|
||||
typename gtsam::traits<CALIBRATION>::TangentVector d;
|
||||
d.setRandom();
|
||||
d *= 0.1;
|
||||
CALIBRATION perturbedCalibration = camera.calibration().retract(d);
|
||||
return PinholeCamera<CALIBRATION>(perturbedCameraPose, perturbedCalibration);
|
||||
}
|
||||
|
||||
template<class CALIBRATION>
|
||||
void projectToMultipleCameras(const PinholeCamera<CALIBRATION>& cam1,
|
||||
const PinholeCamera<CALIBRATION>& cam2,
|
||||
const PinholeCamera<CALIBRATION>& cam3, Point3 landmark,
|
||||
vector<Point2>& measurements_cam) {
|
||||
Point2 cam1_uv1 = cam1.project(landmark);
|
||||
Point2 cam2_uv1 = cam2.project(landmark);
|
||||
Point2 cam3_uv1 = cam3.project(landmark);
|
||||
measurements_cam.push_back(cam1_uv1);
|
||||
measurements_cam.push_back(cam2_uv1);
|
||||
measurements_cam.push_back(cam3_uv1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// default Cal3_S2 cameras
|
||||
namespace vanilla {
|
||||
typedef PinholeCamera<Cal3_S2> Camera;
|
||||
static Cal3_S2 K(fov, w, h);
|
||||
static Cal3_S2 K2(1500, 1200, 0, 640, 480);
|
||||
Camera level_camera(level_pose, K2);
|
||||
Camera level_camera_right(pose_right, K2);
|
||||
Camera cam1(level_pose, K2);
|
||||
Camera cam2(pose_right, K2);
|
||||
Camera cam3(pose_up, K2);
|
||||
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
||||
typedef SmartProjectionCameraFactor<Cal3_S2> SmartFactor;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, perturbCameraPose) {
|
||||
using namespace vanilla;
|
||||
|
|
@ -148,14 +71,14 @@ TEST( SmartProjectionCameraFactor, Constructor2) {
|
|||
TEST( SmartProjectionCameraFactor, Constructor3) {
|
||||
using namespace vanilla;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(measurement1, x1, model);
|
||||
factor1->add(measurement1, x1, unit2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor4) {
|
||||
using namespace vanilla;
|
||||
SmartFactor factor1(rankTol, linThreshold);
|
||||
factor1.add(measurement1, x1, model);
|
||||
factor1.add(measurement1, x1, unit2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -166,17 +89,17 @@ TEST( SmartProjectionCameraFactor, ConstructorWithTransform) {
|
|||
bool enableEPI = false;
|
||||
SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, isImplicit,
|
||||
enableEPI, body_P_sensor1);
|
||||
factor1.add(measurement1, x1, model);
|
||||
factor1.add(measurement1, x1, unit2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Equals ) {
|
||||
using namespace vanilla;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(measurement1, x1, model);
|
||||
factor1->add(measurement1, x1, unit2);
|
||||
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor());
|
||||
factor2->add(measurement1, x1, model);
|
||||
factor2->add(measurement1, x1, unit2);
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
|
|
@ -184,20 +107,13 @@ TEST( SmartProjectionCameraFactor, noiseless ) {
|
|||
// cout << " ************************ SmartProjectionCameraFactor: noisy ****************************" << endl;
|
||||
using namespace vanilla;
|
||||
|
||||
// landmark ~5 meters infront of camera
|
||||
Point3 landmark(5, 0.5, 1.2);
|
||||
|
||||
// 1. Project two landmarks into two cameras and triangulate
|
||||
Point2 level_uv = level_camera.project(landmark);
|
||||
Point2 level_uv_right = level_camera_right.project(landmark);
|
||||
|
||||
Values values;
|
||||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, model);
|
||||
factor1->add(level_uv_right, c2, model);
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
|
||||
double expectedError = 0.0;
|
||||
DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
|
||||
|
|
@ -209,13 +125,10 @@ TEST( SmartProjectionCameraFactor, noisy ) {
|
|||
|
||||
using namespace vanilla;
|
||||
|
||||
// landmark ~5 meters infront of camera
|
||||
Point3 landmark(5, 0.5, 1.2);
|
||||
|
||||
// 1. Project two landmarks into two cameras and triangulate
|
||||
Point2 pixelError(0.2, 0.2);
|
||||
Point2 level_uv = level_camera.project(landmark) + pixelError;
|
||||
Point2 level_uv_right = level_camera_right.project(landmark);
|
||||
Point2 level_uv = level_camera.project(landmark1) + pixelError;
|
||||
Point2 level_uv_right = level_camera_right.project(landmark1);
|
||||
|
||||
Values values;
|
||||
values.insert(c1, level_camera);
|
||||
|
|
@ -223,8 +136,8 @@ TEST( SmartProjectionCameraFactor, noisy ) {
|
|||
values.insert(c2, perturbed_level_camera_right);
|
||||
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, model);
|
||||
factor1->add(level_uv_right, c2, model);
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
|
||||
double actualError1 = factor1->error(values);
|
||||
|
||||
|
|
@ -234,8 +147,8 @@ TEST( SmartProjectionCameraFactor, noisy ) {
|
|||
measurements.push_back(level_uv_right);
|
||||
|
||||
vector<SharedNoiseModel> noises;
|
||||
noises.push_back(model);
|
||||
noises.push_back(model);
|
||||
noises.push_back(unit2);
|
||||
noises.push_back(unit2);
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(c1);
|
||||
|
|
@ -253,11 +166,6 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
|
|||
|
||||
using namespace vanilla;
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
|
|
@ -271,13 +179,13 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
|
|||
views.push_back(c3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
smartFactor1->add(measurements_cam1, views, unit2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
smartFactor2->add(measurements_cam2, views, unit2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
|
||||
|
||||
|
|
@ -327,11 +235,6 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
|
|||
|
||||
using namespace vanilla;
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
|
|
@ -351,7 +254,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
|
|||
track1.measurements.push_back(measures);
|
||||
}
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(track1, model);
|
||||
smartFactor1->add(track1, unit2);
|
||||
|
||||
SfM_Track track2;
|
||||
for (size_t i = 0; i < 3; ++i) {
|
||||
|
|
@ -361,10 +264,10 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
|
|||
track2.measurements.push_back(measures);
|
||||
}
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(track2, model);
|
||||
smartFactor2->add(track2, unit2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
|
||||
|
||||
|
|
@ -414,13 +317,6 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
|
|||
|
||||
using namespace vanilla;
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
Point3 landmark4(10, 0.5, 1.2);
|
||||
Point3 landmark5(10, -0.5, 1.2);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
|
||||
measurements_cam4, measurements_cam5;
|
||||
|
||||
|
|
@ -437,19 +333,19 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
|
|||
views.push_back(c3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
smartFactor1->add(measurements_cam1, views, unit2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
smartFactor2->add(measurements_cam2, views, unit2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
|
||||
smartFactor4->add(measurements_cam4, views, model);
|
||||
smartFactor4->add(measurements_cam4, views, unit2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
|
||||
smartFactor5->add(measurements_cam5, views, model);
|
||||
smartFactor5->add(measurements_cam5, views, unit2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
|
||||
|
||||
|
|
@ -499,33 +395,11 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
|
|||
tictoc_print_();
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
// Cal3Bundler cameras
|
||||
namespace bundler {
|
||||
typedef PinholeCamera<Cal3Bundler> Camera;
|
||||
static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
|
||||
Camera level_camera(level_pose, K);
|
||||
Camera level_camera_right(pose_right, K);
|
||||
Pose3 pose1 = level_pose;
|
||||
Camera cam1(level_pose, K);
|
||||
Camera cam2(pose_right, K);
|
||||
Camera cam3(pose_up, K);
|
||||
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
||||
typedef SmartProjectionCameraFactor<Cal3Bundler> SmartFactorBundler;
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
|
||||
|
||||
using namespace bundler;
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
Point3 landmark4(10, 0.5, 1.2);
|
||||
Point3 landmark5(10, -0.5, 1.2);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
|
||||
measurements_cam4, measurements_cam5;
|
||||
|
||||
|
|
@ -542,19 +416,19 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
|
|||
views.push_back(c3);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler());
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
smartFactor1->add(measurements_cam1, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler());
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
smartFactor2->add(measurements_cam2, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler());
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor4(new SmartFactorBundler());
|
||||
smartFactor4->add(measurements_cam4, views, model);
|
||||
smartFactor4->add(measurements_cam4, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor5(new SmartFactorBundler());
|
||||
smartFactor5->add(measurements_cam5, views, model);
|
||||
smartFactor5->add(measurements_cam5, views, unit2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
|
||||
|
||||
|
|
@ -605,13 +479,6 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
|
|||
|
||||
using namespace bundler;
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
Point3 landmark4(10, 0.5, 1.2);
|
||||
Point3 landmark5(10, -0.5, 1.2);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3,
|
||||
measurements_cam4, measurements_cam5;
|
||||
|
||||
|
|
@ -628,19 +495,19 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
|
|||
views.push_back(c3);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler());
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
smartFactor1->add(measurements_cam1, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler());
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
smartFactor2->add(measurements_cam2, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler());
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor4(new SmartFactorBundler());
|
||||
smartFactor4->add(measurements_cam4, views, model);
|
||||
smartFactor4->add(measurements_cam4, views, unit2);
|
||||
|
||||
SmartFactorBundler::shared_ptr smartFactor5(new SmartFactorBundler());
|
||||
smartFactor5->add(measurements_cam5, views, model);
|
||||
smartFactor5->add(measurements_cam5, views, unit2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
|
||||
|
||||
|
|
@ -690,20 +557,13 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
|
|||
TEST( SmartProjectionCameraFactor, noiselessBundler ) {
|
||||
|
||||
using namespace bundler;
|
||||
// landmark ~5 meters infront of camera
|
||||
Point3 landmark(5, 0.5, 1.2);
|
||||
|
||||
// 1. Project two landmarks into two cameras and triangulate
|
||||
Point2 level_uv = level_camera.project(landmark);
|
||||
Point2 level_uv_right = level_camera_right.project(landmark);
|
||||
|
||||
Values values;
|
||||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
||||
SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
|
||||
factor1->add(level_uv, c1, model);
|
||||
factor1->add(level_uv_right, c2, model);
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
|
||||
double actualError = factor1->error(values);
|
||||
|
||||
|
|
@ -718,28 +578,21 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) {
|
|||
if (factor1->point(values))
|
||||
expectedPoint = *(factor1->point(values));
|
||||
|
||||
EXPECT(assert_equal(expectedPoint, landmark, 1e-3));
|
||||
EXPECT(assert_equal(expectedPoint, landmark1, 1e-3));
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
|
||||
|
||||
using namespace bundler;
|
||||
// landmark ~5 meters infront of camera
|
||||
Point3 landmark(5, 0.5, 1.2);
|
||||
|
||||
// 1. Project two landmarks into two cameras and triangulate
|
||||
Point2 level_uv = level_camera.project(landmark);
|
||||
Point2 level_uv_right = level_camera_right.project(landmark);
|
||||
|
||||
Values values;
|
||||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
||||
NonlinearFactorGraph smartGraph;
|
||||
SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
|
||||
factor1->add(level_uv, c1, model);
|
||||
factor1->add(level_uv_right, c2, model);
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
smartGraph.push_back(factor1);
|
||||
double expectedError = factor1->error(values);
|
||||
double expectedErrorGraph = smartGraph.error(values);
|
||||
|
|
@ -752,8 +605,8 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
|
|||
// 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
|
||||
// value in the generalGrap
|
||||
NonlinearFactorGraph generalGraph;
|
||||
SFMFactor sfm1(level_uv, model, c1, l1);
|
||||
SFMFactor sfm2(level_uv_right, model, c2, l1);
|
||||
SFMFactor sfm1(level_uv, unit2, c1, l1);
|
||||
SFMFactor sfm2(level_uv_right, unit2, c2, l1);
|
||||
generalGraph.push_back(sfm1);
|
||||
generalGraph.push_back(sfm2);
|
||||
values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
|
||||
|
|
@ -773,20 +626,14 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
|
|||
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
||||
|
||||
using namespace bundler;
|
||||
// landmark ~5 meters infront of camera
|
||||
Point3 landmark(5, 0.5, 1.2);
|
||||
// 1. Project two landmarks into two cameras and triangulate
|
||||
Point2 level_uv = level_camera.project(landmark);
|
||||
Point2 level_uv_right = level_camera_right.project(landmark);
|
||||
|
||||
Values values;
|
||||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
||||
NonlinearFactorGraph smartGraph;
|
||||
SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
|
||||
factor1->add(level_uv, c1, model);
|
||||
factor1->add(level_uv_right, c2, model);
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
smartGraph.push_back(factor1);
|
||||
Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
|
||||
Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
|
||||
|
|
@ -798,8 +645,8 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
|||
// 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
|
||||
// value in the generalGrap
|
||||
NonlinearFactorGraph generalGraph;
|
||||
SFMFactor sfm1(level_uv, model, c1, l1);
|
||||
SFMFactor sfm2(level_uv_right, model, c2, l1);
|
||||
SFMFactor sfm1(level_uv, unit2, c1, l1);
|
||||
SFMFactor sfm2(level_uv_right, unit2, c2, l1);
|
||||
generalGraph.push_back(sfm1);
|
||||
generalGraph.push_back(sfm2);
|
||||
values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
|
||||
|
|
@ -822,20 +669,14 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
|||
// Have to think about how to compare multiplyHessianAdd in generalSfMFactor and smartFactors
|
||||
//TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor2 ){
|
||||
//
|
||||
// // landmark ~5 meters infront of camera
|
||||
// Point3 landmark(5, 0.5, 1.2);
|
||||
// // 1. Project two landmarks into two cameras
|
||||
// Point2 level_uv = level_camera.project(landmark);
|
||||
// Point2 level_uv_right = level_camera_right.project(landmark);
|
||||
//
|
||||
// Values values;
|
||||
// values.insert(c1, level_camera);
|
||||
// values.insert(c2, level_camera_right);
|
||||
//
|
||||
// NonlinearFactorGraph smartGraph;
|
||||
// SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
|
||||
// factor1->add(level_uv, c1, model);
|
||||
// factor1->add(level_uv_right, c2, model);
|
||||
// factor1->add(level_uv, c1, unit2);
|
||||
// factor1->add(level_uv_right, c2, unit2);
|
||||
// smartGraph.push_back(factor1);
|
||||
// GaussianFactorGraph::shared_ptr gfgSmart = smartGraph.linearize(values);
|
||||
//
|
||||
|
|
@ -847,8 +688,8 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
|||
// // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
|
||||
// // value in the generalGrap
|
||||
// NonlinearFactorGraph generalGraph;
|
||||
// SFMFactor sfm1(level_uv, model, c1, l1);
|
||||
// SFMFactor sfm2(level_uv_right, model, c2, l1);
|
||||
// SFMFactor sfm1(level_uv, unit2, c1, l1);
|
||||
// SFMFactor sfm2(level_uv_right, unit2, c2, l1);
|
||||
// generalGraph.push_back(sfm1);
|
||||
// generalGraph.push_back(sfm2);
|
||||
// values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
|
||||
|
|
@ -877,19 +718,13 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
|||
TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
|
||||
|
||||
using namespace bundler;
|
||||
// landmark ~5 meters infront of camera
|
||||
Point3 landmark(5, 0.5, 1.2);
|
||||
// 1. Project two landmarks into two cameras and triangulate
|
||||
Point2 level_uv = level_camera.project(landmark);
|
||||
Point2 level_uv_right = level_camera_right.project(landmark);
|
||||
|
||||
Values values;
|
||||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
||||
SmartFactorBundler::shared_ptr factor1(new SmartFactorBundler());
|
||||
factor1->add(level_uv, c1, model);
|
||||
factor1->add(level_uv_right, c2, model);
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
Matrix expectedF, expectedE;
|
||||
Vector expectedb;
|
||||
|
||||
|
|
@ -904,8 +739,8 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
|
|||
factor1->computeJacobians(expectedF, expectedE, expectedb, cameras);
|
||||
|
||||
NonlinearFactorGraph generalGraph;
|
||||
SFMFactor sfm1(level_uv, model, c1, l1);
|
||||
SFMFactor sfm2(level_uv_right, model, c2, l1);
|
||||
SFMFactor sfm1(level_uv, unit2, c1, l1);
|
||||
SFMFactor sfm2(level_uv_right, unit2, c2, l1);
|
||||
generalGraph.push_back(sfm1);
|
||||
generalGraph.push_back(sfm2);
|
||||
values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
|
||||
|
|
@ -921,12 +756,6 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
|
|||
|
||||
using namespace bundler;
|
||||
|
||||
// landmark ~5 meters infront of camera
|
||||
Point3 landmark(5, 0.5, 1.2);
|
||||
// 1. Project two landmarks into two cameras and triangulate
|
||||
Point2 level_uv = level_camera.project(landmark);
|
||||
Point2 level_uv_right = level_camera_right.project(landmark);
|
||||
|
||||
Values values;
|
||||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
|
@ -940,8 +769,8 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
|
|||
SmartFactorBundler::shared_ptr explicitFactor(
|
||||
new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy, useEPI,
|
||||
isImplicit));
|
||||
explicitFactor->add(level_uv, c1, model);
|
||||
explicitFactor->add(level_uv_right, c2, model);
|
||||
explicitFactor->add(level_uv, c1, unit2);
|
||||
explicitFactor->add(level_uv_right, c2, unit2);
|
||||
|
||||
GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(
|
||||
values);
|
||||
|
|
@ -953,8 +782,8 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
|
|||
SmartFactorBundler::shared_ptr implicitFactor(
|
||||
new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy, useEPI,
|
||||
isImplicit));
|
||||
implicitFactor->add(level_uv, c1, model);
|
||||
implicitFactor->add(level_uv_right, c2, model);
|
||||
implicitFactor->add(level_uv, c1, unit2);
|
||||
implicitFactor->add(level_uv_right, c2, unit2);
|
||||
GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
|
||||
implicitFactor->linearize(values);
|
||||
typedef RegularImplicitSchurFactor<9> Implicit9;
|
||||
|
|
|
|||
Loading…
Reference in New Issue