Merged in feature/smartFactorNoiseCleanup (pull request #183)
SmartFactor noise model in constructorrelease/4.3a0
Chris Beall
commit
c73b835848
|
|
@ -58,7 +58,7 @@ int main(int argc, char* argv[]) {
|
|||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
|
||||
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
|
||||
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
|
||||
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
|
||||
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
|
||||
|
|
@ -71,7 +71,7 @@ int main(int argc, char* argv[]) {
|
|||
// 2. the corresponding camera's key
|
||||
// 3. camera noise model
|
||||
// 4. camera calibration
|
||||
smartfactor->add(measurement, i, measurementNoise);
|
||||
smartfactor->add(measurement, i);
|
||||
}
|
||||
|
||||
// insert the smart factor in the graph
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ int main(int argc, char* argv[]) {
|
|||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
|
||||
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
|
||||
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
|
||||
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
|
||||
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
|
||||
|
|
@ -63,7 +63,7 @@ int main(int argc, char* argv[]) {
|
|||
Point2 measurement = camera.project(points[j]);
|
||||
|
||||
// call add() function to add measurement into a single factor, here we need to add:
|
||||
smartfactor->add(measurement, i, measurementNoise);
|
||||
smartfactor->add(measurement, i);
|
||||
}
|
||||
|
||||
// insert the smart factor in the graph
|
||||
|
|
|
|||
12
gtsam.h
12
gtsam.h
|
|
@ -2364,15 +2364,17 @@ class SmartProjectionParams {
|
|||
template<CALIBRATION>
|
||||
virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
|
||||
|
||||
SmartProjectionPoseFactor(const CALIBRATION* K);
|
||||
SmartProjectionPoseFactor(const CALIBRATION* K,
|
||||
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
|
||||
const CALIBRATION* K);
|
||||
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
|
||||
const CALIBRATION* K,
|
||||
const gtsam::Pose3& body_P_sensor);
|
||||
SmartProjectionPoseFactor(const CALIBRATION* K,
|
||||
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
|
||||
const CALIBRATION* K,
|
||||
const gtsam::Pose3& body_P_sensor,
|
||||
const gtsam::SmartProjectionParams& params);
|
||||
|
||||
void add(const gtsam::Point2& measured_i, size_t poseKey_i,
|
||||
const gtsam::noiseModel::Base* noise_i);
|
||||
void add(const gtsam::Point2& measured_i, size_t poseKey_i);
|
||||
|
||||
// enabling serialization functionality
|
||||
//void serialize() const;
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
* @author Luca Carlone
|
||||
* @author Zsolt Kira
|
||||
* @author Frank Dellaert
|
||||
* @author Chris Beall
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -79,22 +80,6 @@ protected:
|
|||
typedef Eigen::Matrix<double, Dim, 1> VectorD;
|
||||
typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
|
||||
|
||||
// check that noise model is isotropic and the same
|
||||
// TODO, this is hacky, we should just do this via constructor, not add
|
||||
void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) {
|
||||
if (!sharedNoiseModel)
|
||||
return;
|
||||
SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
|
||||
noiseModel::Isotropic>(sharedNoiseModel);
|
||||
if (!sharedIsotropic)
|
||||
throw std::runtime_error("SmartFactorBase: needs isotropic");
|
||||
if (!noiseModel_)
|
||||
noiseModel_ = sharedIsotropic;
|
||||
else if (!sharedIsotropic->equals(*noiseModel_))
|
||||
throw std::runtime_error(
|
||||
"SmartFactorBase: cannot add measurements with different noise model");
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
// Definitions for blocks of F, externally visible
|
||||
|
|
@ -109,8 +94,21 @@ public:
|
|||
typedef CameraSet<CAMERA> Cameras;
|
||||
|
||||
/// Constructor
|
||||
SmartFactorBase(boost::optional<Pose3> body_P_sensor = boost::none) :
|
||||
body_P_sensor_(body_P_sensor){}
|
||||
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
|
||||
boost::optional<Pose3> body_P_sensor = boost::none) :
|
||||
body_P_sensor_(body_P_sensor){
|
||||
|
||||
if (!sharedNoiseModel)
|
||||
throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
|
||||
|
||||
SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
|
||||
noiseModel::Isotropic>(sharedNoiseModel);
|
||||
|
||||
if (!sharedIsotropic)
|
||||
throw std::runtime_error("SmartFactorBase: needs isotropic");
|
||||
|
||||
noiseModel_ = sharedIsotropic;
|
||||
}
|
||||
|
||||
/// Virtual destructor, subclasses from NonlinearFactor
|
||||
virtual ~SmartFactorBase() {
|
||||
|
|
@ -122,34 +120,18 @@ public:
|
|||
* @param poseKey is the index corresponding to the camera observing the landmark
|
||||
* @param sharedNoiseModel is the measurement noise
|
||||
*/
|
||||
void add(const Z& measured_i, const Key& cameraKey_i,
|
||||
const SharedNoiseModel& sharedNoiseModel) {
|
||||
void add(const Z& measured_i, const Key& cameraKey_i) {
|
||||
this->measured_.push_back(measured_i);
|
||||
this->keys_.push_back(cameraKey_i);
|
||||
maybeSetNoiseModel(sharedNoiseModel);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a bunch of measurements, together with the camera keys and noises
|
||||
* Add a bunch of measurements, together with the camera keys
|
||||
*/
|
||||
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
|
||||
std::vector<SharedNoiseModel>& noises) {
|
||||
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys) {
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
this->measured_.push_back(measurements.at(i));
|
||||
this->keys_.push_back(cameraKeys.at(i));
|
||||
maybeSetNoiseModel(noises.at(i));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a bunch of measurements and use the same noise model for all of them
|
||||
*/
|
||||
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
|
||||
const SharedNoiseModel& noise) {
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
this->measured_.push_back(measurements.at(i));
|
||||
this->keys_.push_back(cameraKeys.at(i));
|
||||
maybeSetNoiseModel(noise);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -158,11 +140,10 @@ public:
|
|||
* The noise is assumed to be the same for all measurements
|
||||
*/
|
||||
template<class SFM_TRACK>
|
||||
void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) {
|
||||
void add(const SFM_TRACK& trackToAdd) {
|
||||
for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
|
||||
this->measured_.push_back(trackToAdd.measurements[k].second);
|
||||
this->keys_.push_back(trackToAdd.measurements[k].first);
|
||||
maybeSetNoiseModel(noise);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -159,10 +159,10 @@ public:
|
|||
* @param body_P_sensor pose of the camera in the body frame
|
||||
* @param params internal parameters of the smart factors
|
||||
*/
|
||||
SmartProjectionFactor(
|
||||
SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
|
||||
const boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
const SmartProjectionParams& params = SmartProjectionParams()) :
|
||||
Base(body_P_sensor), params_(params), //
|
||||
Base(sharedNoiseModel, body_P_sensor), params_(params), //
|
||||
result_(TriangulationResult::Degenerate()) {
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -61,14 +61,16 @@ public:
|
|||
|
||||
/**
|
||||
* Constructor
|
||||
* @param Isotropic measurement noise
|
||||
* @param K (fixed) calibration, assumed to be the same for all cameras
|
||||
* @param body_P_sensor pose of the camera in the body frame
|
||||
* @param params internal parameters of the smart factors
|
||||
*/
|
||||
SmartProjectionPoseFactor(const boost::shared_ptr<CALIBRATION> K,
|
||||
SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
|
||||
const boost::shared_ptr<CALIBRATION> K,
|
||||
const boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
const SmartProjectionParams& params = SmartProjectionParams()) :
|
||||
Base(body_P_sensor, params), K_(K) {
|
||||
Base(sharedNoiseModel, body_P_sensor, params), K_(K) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
|
|
|
|||
|
|
@ -16,17 +16,24 @@
|
|||
* @date Feb 2015
|
||||
*/
|
||||
|
||||
#include "../SmartFactorBase.h"
|
||||
#include <gtsam/slam/SmartFactorBase.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
|
||||
static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
|
||||
|
||||
/* ************************************************************************* */
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
|
||||
public:
|
||||
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
|
||||
PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
|
||||
}
|
||||
virtual double error(const Values& values) const {
|
||||
return 0.0;
|
||||
}
|
||||
|
|
@ -37,15 +44,19 @@ class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
|
|||
};
|
||||
|
||||
TEST(SmartFactorBase, Pinhole) {
|
||||
PinholeFactor f;
|
||||
f.add(Point2(), 1, SharedNoiseModel());
|
||||
f.add(Point2(), 2, SharedNoiseModel());
|
||||
PinholeFactor f= PinholeFactor(unit2);
|
||||
f.add(Point2(), 1);
|
||||
f.add(Point2(), 2);
|
||||
EXPECT_LONGS_EQUAL(2 * 2, f.dim());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#include <gtsam/geometry/StereoCamera.h>
|
||||
class StereoFactor: public SmartFactorBase<StereoCamera> {
|
||||
public:
|
||||
typedef SmartFactorBase<StereoCamera> Base;
|
||||
StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
|
||||
}
|
||||
virtual double error(const Values& values) const {
|
||||
return 0.0;
|
||||
}
|
||||
|
|
@ -56,9 +67,9 @@ class StereoFactor: public SmartFactorBase<StereoCamera> {
|
|||
};
|
||||
|
||||
TEST(SmartFactorBase, Stereo) {
|
||||
StereoFactor f;
|
||||
f.add(StereoPoint2(), 1, SharedNoiseModel());
|
||||
f.add(StereoPoint2(), 2, SharedNoiseModel());
|
||||
StereoFactor f(unit3);
|
||||
f.add(StereoPoint2(), 1);
|
||||
f.add(StereoPoint2(), 2);
|
||||
EXPECT_LONGS_EQUAL(2 * 3, f.dim());
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -72,39 +72,39 @@ TEST( SmartProjectionCameraFactor, perturbCameraPose) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor) {
|
||||
using namespace vanilla;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor2) {
|
||||
using namespace vanilla;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(boost::none, params);
|
||||
SmartFactor factor1(unit2, boost::none, params);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor3) {
|
||||
using namespace vanilla;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(measurement1, x1, unit2);
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
factor1->add(measurement1, x1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor4) {
|
||||
using namespace vanilla;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(boost::none, params);
|
||||
factor1.add(measurement1, x1, unit2);
|
||||
SmartFactor factor1(unit2, boost::none, params);
|
||||
factor1.add(measurement1, x1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Equals ) {
|
||||
using namespace vanilla;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(measurement1, x1, unit2);
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
factor1->add(measurement1, x1);
|
||||
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor());
|
||||
factor2->add(measurement1, x1, unit2);
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
|
||||
factor2->add(measurement1, x1);
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
|
|
@ -115,9 +115,9 @@ TEST( SmartProjectionCameraFactor, noiseless ) {
|
|||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
factor1->add(level_uv, c1);
|
||||
factor1->add(level_uv_right, c2);
|
||||
|
||||
double expectedError = 0.0;
|
||||
DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
|
||||
|
|
@ -140,9 +140,9 @@ TEST( SmartProjectionCameraFactor, noisy ) {
|
|||
Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right);
|
||||
values.insert(c2, perturbed_level_camera_right);
|
||||
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
factor1->add(level_uv, c1);
|
||||
factor1->add(level_uv_right, c2);
|
||||
|
||||
// Point is now uninitialized before a triangulation event
|
||||
EXPECT(!factor1->point());
|
||||
|
|
@ -164,20 +164,16 @@ TEST( SmartProjectionCameraFactor, noisy ) {
|
|||
Vector actual = factor1->whitenedError(cameras1, point1);
|
||||
EXPECT(assert_equal(expected, actual, 1));
|
||||
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor());
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
|
||||
vector<Point2> measurements;
|
||||
measurements.push_back(level_uv);
|
||||
measurements.push_back(level_uv_right);
|
||||
|
||||
vector<SharedNoiseModel> noises;
|
||||
noises.push_back(unit2);
|
||||
noises.push_back(unit2);
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(c1);
|
||||
views.push_back(c2);
|
||||
|
||||
factor2->add(measurements, views, noises);
|
||||
factor2->add(measurements, views);
|
||||
|
||||
double actualError2 = factor2->error(values);
|
||||
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1);
|
||||
|
|
@ -195,16 +191,16 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
// Create and fill smartfactors
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
|
||||
vector<Key> views;
|
||||
views.push_back(c1);
|
||||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
smartFactor1->add(measurements_cam1, views, unit2);
|
||||
smartFactor2->add(measurements_cam2, views, unit2);
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
// Create factor graph and add priors on two cameras
|
||||
NonlinearFactorGraph graph;
|
||||
|
|
@ -308,8 +304,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
|
|||
measures.second = measurements_cam1.at(i);
|
||||
track1.measurements.push_back(measures);
|
||||
}
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(track1, unit2);
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
|
||||
smartFactor1->add(track1);
|
||||
|
||||
SfM_Track track2;
|
||||
for (size_t i = 0; i < 3; ++i) {
|
||||
|
|
@ -318,11 +314,11 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
|
|||
measures.second = measurements_cam2.at(i);
|
||||
track2.measurements.push_back(measures);
|
||||
}
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(track2, unit2);
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
|
||||
smartFactor2->add(track2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
|
||||
|
||||
|
|
@ -384,20 +380,20 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
|
|||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(measurements_cam1, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(measurements_cam2, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
|
||||
smartFactor4->add(measurements_cam4, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
|
||||
smartFactor4->add(measurements_cam4, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
|
||||
smartFactor5->add(measurements_cam5, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
|
||||
smartFactor5->add(measurements_cam5, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
|
||||
|
||||
|
|
@ -464,20 +460,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
|
|||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(measurements_cam1, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(measurements_cam2, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
|
||||
smartFactor4->add(measurements_cam4, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
|
||||
smartFactor4->add(measurements_cam4, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
|
||||
smartFactor5->add(measurements_cam5, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
|
||||
smartFactor5->add(measurements_cam5, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
|
||||
|
||||
|
|
@ -540,20 +536,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
|
|||
views.push_back(c2);
|
||||
views.push_back(c3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
|
||||
smartFactor1->add(measurements_cam1, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
|
||||
smartFactor2->add(measurements_cam2, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
|
||||
smartFactor3->add(measurements_cam3, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
|
||||
smartFactor4->add(measurements_cam4, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
|
||||
smartFactor4->add(measurements_cam4, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
|
||||
smartFactor5->add(measurements_cam5, views, unit2);
|
||||
SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
|
||||
smartFactor5->add(measurements_cam5, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
|
||||
|
||||
|
|
@ -604,9 +600,9 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) {
|
|||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
factor1->add(level_uv, c1);
|
||||
factor1->add(level_uv_right, c2);
|
||||
|
||||
double actualError = factor1->error(values);
|
||||
|
||||
|
|
@ -633,9 +629,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
|
|||
values.insert(c2, level_camera_right);
|
||||
|
||||
NonlinearFactorGraph smartGraph;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
factor1->add(level_uv, c1);
|
||||
factor1->add(level_uv_right, c2);
|
||||
smartGraph.push_back(factor1);
|
||||
double expectedError = factor1->error(values);
|
||||
double expectedErrorGraph = smartGraph.error(values);
|
||||
|
|
@ -674,9 +670,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
|
|||
values.insert(c2, level_camera_right);
|
||||
|
||||
NonlinearFactorGraph smartGraph;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
factor1->add(level_uv, c1);
|
||||
factor1->add(level_uv_right, c2);
|
||||
smartGraph.push_back(factor1);
|
||||
Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
|
||||
Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
|
||||
|
|
@ -765,9 +761,9 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
|
|||
values.insert(c1, level_camera);
|
||||
values.insert(c2, level_camera_right);
|
||||
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor());
|
||||
factor1->add(level_uv, c1, unit2);
|
||||
factor1->add(level_uv_right, c2, unit2);
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
|
||||
factor1->add(level_uv, c1);
|
||||
factor1->add(level_uv_right, c2);
|
||||
Matrix expectedE;
|
||||
Vector expectedb;
|
||||
|
||||
|
|
@ -814,9 +810,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
|
|||
params.setEnableEPI(useEPI);
|
||||
|
||||
SmartFactor::shared_ptr explicitFactor(
|
||||
new SmartFactor(boost::none, params));
|
||||
explicitFactor->add(level_uv, c1, unit2);
|
||||
explicitFactor->add(level_uv_right, c2, unit2);
|
||||
new SmartFactor(unit2, boost::none, params));
|
||||
explicitFactor->add(level_uv, c1);
|
||||
explicitFactor->add(level_uv_right, c2);
|
||||
|
||||
GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(
|
||||
values);
|
||||
|
|
@ -826,9 +822,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
|
|||
// Implicit Schur version
|
||||
params.setLinearizationMode(gtsam::IMPLICIT_SCHUR);
|
||||
SmartFactor::shared_ptr implicitFactor(
|
||||
new SmartFactor(boost::none, params));
|
||||
implicitFactor->add(level_uv, c1, unit2);
|
||||
implicitFactor->add(level_uv_right, c2, unit2);
|
||||
new SmartFactor(unit2, boost::none, params));
|
||||
implicitFactor->add(level_uv, c1);
|
||||
implicitFactor->add(level_uv_right, c2);
|
||||
GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
|
||||
implicitFactor->linearize(values);
|
||||
CHECK(gaussianImplicitSchurFactor);
|
||||
|
|
|
|||
|
|
@ -53,7 +53,7 @@ LevenbergMarquardtParams lmParams;
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Constructor) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -61,14 +61,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) {
|
|||
using namespace vanillaPose;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(sharedK, boost::none, params);
|
||||
SmartFactor factor1(model, sharedK, boost::none, params);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Constructor3) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
|
||||
factor1->add(measurement1, x1, model);
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
|
||||
factor1->add(measurement1, x1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -76,18 +76,18 @@ TEST( SmartProjectionPoseFactor, Constructor4) {
|
|||
using namespace vanillaPose;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(sharedK, boost::none, params);
|
||||
factor1.add(measurement1, x1, model);
|
||||
SmartFactor factor1(model, sharedK, boost::none, params);
|
||||
factor1.add(measurement1, x1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Equals ) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
|
||||
factor1->add(measurement1, x1, model);
|
||||
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
|
||||
factor1->add(measurement1, x1);
|
||||
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor(sharedK));
|
||||
factor2->add(measurement1, x1, model);
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK));
|
||||
factor2->add(measurement1, x1);
|
||||
|
||||
CHECK(assert_equal(*factor1, *factor2));
|
||||
}
|
||||
|
|
@ -101,9 +101,9 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
|
|||
Point2 level_uv = level_camera.project(landmark1);
|
||||
Point2 level_uv_right = level_camera_right.project(landmark1);
|
||||
|
||||
SmartFactor factor(sharedK);
|
||||
factor.add(level_uv, x1, model);
|
||||
factor.add(level_uv_right, x2, model);
|
||||
SmartFactor factor(model, sharedK);
|
||||
factor.add(level_uv, x1);
|
||||
factor.add(level_uv_right, x2);
|
||||
|
||||
Values values; // it's a pose factor, hence these are poses
|
||||
values.insert(x1, cam1.pose());
|
||||
|
|
@ -166,26 +166,22 @@ TEST( SmartProjectionPoseFactor, noisy ) {
|
|||
Point3(0.5, 0.1, 0.3));
|
||||
values.insert(x2, pose_right.compose(noise_pose));
|
||||
|
||||
SmartFactor::shared_ptr factor(new SmartFactor((sharedK)));
|
||||
factor->add(level_uv, x1, model);
|
||||
factor->add(level_uv_right, x2, model);
|
||||
SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK));
|
||||
factor->add(level_uv, x1);
|
||||
factor->add(level_uv_right, x2);
|
||||
|
||||
double actualError1 = factor->error(values);
|
||||
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor((sharedK)));
|
||||
SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK));
|
||||
vector<Point2> measurements;
|
||||
measurements.push_back(level_uv);
|
||||
measurements.push_back(level_uv_right);
|
||||
|
||||
vector<SharedNoiseModel> noises;
|
||||
noises.push_back(model);
|
||||
noises.push_back(model);
|
||||
|
||||
vector<Key> views;
|
||||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
|
||||
factor2->add(measurements, views, noises);
|
||||
factor2->add(measurements, views);
|
||||
double actualError2 = factor2->error(values);
|
||||
DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
|
||||
}
|
||||
|
|
@ -238,14 +234,14 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
|
|||
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
||||
params.setEnableEPI(false);
|
||||
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor1(K, sensor_to_body, params);
|
||||
smartFactor1.add(measurements_cam1, views, model);
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor1(model, K, sensor_to_body, params);
|
||||
smartFactor1.add(measurements_cam1, views);
|
||||
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor2(K, sensor_to_body, params);
|
||||
smartFactor2.add(measurements_cam2, views, model);
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor2(model, K, sensor_to_body, params);
|
||||
smartFactor2.add(measurements_cam2, views);
|
||||
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor3(K, sensor_to_body, params);
|
||||
smartFactor3.add(measurements_cam3, views, model);
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor3(model, K, sensor_to_body, params);
|
||||
smartFactor3.add(measurements_cam3, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -296,14 +292,14 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK2));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK2));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -366,8 +362,8 @@ TEST( SmartProjectionPoseFactor, Factors ) {
|
|||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(sharedK);
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK);
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::Cameras cameras;
|
||||
cameras.push_back(cam1);
|
||||
|
|
@ -524,14 +520,14 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -586,19 +582,19 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
|
|||
params.setLinearizationMode(gtsam::JACOBIAN_SVD);
|
||||
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
||||
params.setEnableEPI(false);
|
||||
SmartFactor factor1(sharedK, boost::none, params);
|
||||
SmartFactor factor1(model, sharedK, boost::none, params);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -650,16 +646,16 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
|
|||
params.setEnableEPI(false);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -717,20 +713,20 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor4->add(measurements_cam4, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor4->add(measurements_cam4, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -775,16 +771,16 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
|
|||
params.setLinearizationMode(gtsam::JACOBIAN_Q);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -895,16 +891,16 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
|
|||
params.setRankTolerance(10);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
|
|
@ -978,12 +974,12 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
|
|||
params.setDegeneracyMode(gtsam::HANDLE_INFINITY);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK2, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
new SmartFactor(model, sharedK2, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK2, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
new SmartFactor(model, sharedK2, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
|
||||
|
|
@ -1040,16 +1036,16 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
|
|||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
new SmartFactor(model, sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
|
||||
|
|
@ -1108,8 +1104,8 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
|
|||
measurements_cam1.push_back(cam1_uv1);
|
||||
measurements_cam1.push_back(cam2_uv1);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
|
||||
Point3(0.5, 0.1, 0.3));
|
||||
|
|
@ -1140,8 +1136,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
|
|||
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
|
||||
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(sharedK));
|
||||
smartFactorInstance->add(measurements_cam1, views, model);
|
||||
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK));
|
||||
smartFactorInstance->add(measurements_cam1, views);
|
||||
|
||||
Values values;
|
||||
values.insert(x1, cam1.pose());
|
||||
|
|
@ -1196,8 +1192,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
|||
vector<Point2> measurements_cam1;
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor(new SmartFactor(sharedK2));
|
||||
smartFactor->add(measurements_cam1, views, model);
|
||||
SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2));
|
||||
smartFactor->add(measurements_cam1, views);
|
||||
|
||||
Values values;
|
||||
values.insert(x1, cam1.pose());
|
||||
|
|
@ -1239,8 +1235,8 @@ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
|
|||
using namespace bundlerPose;
|
||||
SmartProjectionParams params;
|
||||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
SmartFactor factor(sharedBundlerK, boost::none, params);
|
||||
factor.add(measurement1, x1, model);
|
||||
SmartFactor factor(model, sharedBundlerK, boost::none, params);
|
||||
factor.add(measurement1, x1);
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
|
|
@ -1263,14 +1259,14 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
|
|||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedBundlerK));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedBundlerK));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedBundlerK));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -1334,16 +1330,16 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
|
|||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedBundlerK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
new SmartFactor(model, sharedBundlerK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedBundlerK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
new SmartFactor(model, sharedBundlerK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedBundlerK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
new SmartFactor(model, sharedBundlerK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
|
||||
|
|
|
|||
|
|
@ -87,17 +87,17 @@ int main(int argc, char** argv){
|
|||
|
||||
//read stereo measurements and construct smart factors
|
||||
|
||||
SmartFactor::shared_ptr factor(new SmartFactor(K));
|
||||
SmartFactor::shared_ptr factor(new SmartFactor(model, K));
|
||||
size_t current_l = 3; // hardcoded landmark ID from first measurement
|
||||
|
||||
while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) {
|
||||
|
||||
if(current_l != l) {
|
||||
graph.push_back(factor);
|
||||
factor = SmartFactor::shared_ptr(new SmartFactor(K));
|
||||
factor = SmartFactor::shared_ptr(new SmartFactor(model, K));
|
||||
current_l = l;
|
||||
}
|
||||
factor->add(Point2(uL,v), i, model);
|
||||
factor->add(Point2(uL,v), i);
|
||||
}
|
||||
|
||||
Pose3 firstPose = initial_estimate.at<Pose3>(1);
|
||||
|
|
|
|||
|
|
@ -109,17 +109,17 @@ int main(int argc, char** argv){
|
|||
|
||||
//read stereo measurements and construct smart factors
|
||||
|
||||
SmartFactor::shared_ptr factor(new SmartFactor());
|
||||
SmartFactor::shared_ptr factor(new SmartFactor(model));
|
||||
size_t current_l = 3; // hardcoded landmark ID from first measurement
|
||||
|
||||
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
|
||||
|
||||
if(current_l != l) {
|
||||
graph.push_back(factor);
|
||||
factor = SmartFactor::shared_ptr(new SmartFactor());
|
||||
factor = SmartFactor::shared_ptr(new SmartFactor(model));
|
||||
current_l = l;
|
||||
}
|
||||
factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K);
|
||||
factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K);
|
||||
}
|
||||
|
||||
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
|
||||
|
|
|
|||
|
|
@ -159,8 +159,10 @@ public:
|
|||
* Constructor
|
||||
* @param params internal parameters of the smart factors
|
||||
*/
|
||||
SmartStereoProjectionFactor(const SmartStereoProjectionParams& params =
|
||||
SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
|
||||
const SmartStereoProjectionParams& params =
|
||||
SmartStereoProjectionParams()) :
|
||||
Base(sharedNoiseModel), //
|
||||
params_(params), //
|
||||
result_(TriangulationResult::Degenerate()) {
|
||||
}
|
||||
|
|
|
|||
|
|
@ -62,15 +62,13 @@ public:
|
|||
|
||||
/**
|
||||
* Constructor
|
||||
* @param rankTol tolerance used to check if point triangulation is degenerate
|
||||
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
|
||||
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
|
||||
* otherwise the factor is simply neglected
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
* @param Isotropic measurement noise
|
||||
* @param params internal parameters of the smart factors
|
||||
*/
|
||||
SmartStereoProjectionPoseFactor(const SmartStereoProjectionParams& params =
|
||||
SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
|
||||
const SmartStereoProjectionParams& params =
|
||||
SmartStereoProjectionParams()) :
|
||||
Base(params) {
|
||||
Base(sharedNoiseModel, params) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
|
|
@ -80,27 +78,23 @@ public:
|
|||
* add a new measurement and pose key
|
||||
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
|
||||
* @param poseKey is key corresponding to the camera observing the same landmark
|
||||
* @param noise_i is the measurement noise
|
||||
* @param K_i is the (known) camera calibration
|
||||
* @param K is the (fixed) camera calibration
|
||||
*/
|
||||
void add(const StereoPoint2 measured_i, const Key poseKey_i,
|
||||
const SharedNoiseModel noise_i,
|
||||
const boost::shared_ptr<Cal3_S2Stereo> K_i) {
|
||||
Base::add(measured_i, poseKey_i, noise_i);
|
||||
K_all_.push_back(K_i);
|
||||
void add(const StereoPoint2 measured, const Key poseKey,
|
||||
const boost::shared_ptr<Cal3_S2Stereo> K) {
|
||||
Base::add(measured, poseKey);
|
||||
K_all_.push_back(K);
|
||||
}
|
||||
|
||||
/**
|
||||
* Variant of the previous one in which we include a set of measurements
|
||||
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
|
||||
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
|
||||
* @param noises vector of measurement noises
|
||||
* @param Ks vector of calibration objects
|
||||
*/
|
||||
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
|
||||
std::vector<SharedNoiseModel> noises,
|
||||
std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
|
||||
Base::add(measurements, poseKeys, noises);
|
||||
Base::add(measurements, poseKeys);
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
K_all_.push_back(Ks.at(i));
|
||||
}
|
||||
|
|
@ -110,13 +104,12 @@ public:
|
|||
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
|
||||
* @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
|
||||
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
|
||||
* @param noise measurement noise (same for all measurements)
|
||||
* @param K the (known) camera calibration (same for all measurements)
|
||||
*/
|
||||
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
|
||||
const SharedNoiseModel noise, const boost::shared_ptr<Cal3_S2Stereo> K) {
|
||||
const boost::shared_ptr<Cal3_S2Stereo> K) {
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
Base::add(measurements.at(i), poseKeys.at(i), noise);
|
||||
Base::add(measurements.at(i), poseKeys.at(i));
|
||||
K_all_.push_back(K);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -81,33 +81,33 @@ LevenbergMarquardtParams lm_params;
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartStereoProjectionPoseFactor, Constructor) {
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor());
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartStereoProjectionPoseFactor, Constructor2) {
|
||||
SmartStereoProjectionPoseFactor factor1(params);
|
||||
SmartStereoProjectionPoseFactor factor1(model, params);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartStereoProjectionPoseFactor, Constructor3) {
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor());
|
||||
factor1->add(measurement1, poseKey1, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
|
||||
factor1->add(measurement1, poseKey1, K);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartStereoProjectionPoseFactor, Constructor4) {
|
||||
SmartStereoProjectionPoseFactor factor1(params);
|
||||
factor1.add(measurement1, poseKey1, model, K);
|
||||
SmartStereoProjectionPoseFactor factor1(model, params);
|
||||
factor1.add(measurement1, poseKey1, K);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartStereoProjectionPoseFactor, Equals ) {
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor());
|
||||
factor1->add(measurement1, poseKey1, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
|
||||
factor1->add(measurement1, poseKey1, K);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor());
|
||||
factor2->add(measurement1, poseKey1, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
|
||||
factor2->add(measurement1, poseKey1, K);
|
||||
|
||||
CHECK(assert_equal(*factor1, *factor2));
|
||||
}
|
||||
|
|
@ -134,9 +134,9 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
|
|||
values.insert(x1, level_pose);
|
||||
values.insert(x2, level_pose_right);
|
||||
|
||||
SmartStereoProjectionPoseFactor factor1;
|
||||
factor1.add(level_uv, x1, model, K2);
|
||||
factor1.add(level_uv_right, x2, model, K2);
|
||||
SmartStereoProjectionPoseFactor factor1(model);
|
||||
factor1.add(level_uv, x1, K2);
|
||||
factor1.add(level_uv_right, x2, K2);
|
||||
|
||||
double actualError = factor1.error(values);
|
||||
double expectedError = 0.0;
|
||||
|
|
@ -176,21 +176,17 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
|
|||
Point3(0.5, 0.1, 0.3));
|
||||
values.insert(x2, level_pose_right.compose(noise_pose));
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor());
|
||||
factor1->add(level_uv, x1, model, K);
|
||||
factor1->add(level_uv_right, x2, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
|
||||
factor1->add(level_uv, x1, K);
|
||||
factor1->add(level_uv_right, x2, K);
|
||||
|
||||
double actualError1 = factor1->error(values);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor());
|
||||
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
|
||||
vector<StereoPoint2> measurements;
|
||||
measurements.push_back(level_uv);
|
||||
measurements.push_back(level_uv_right);
|
||||
|
||||
vector<SharedNoiseModel> noises;
|
||||
noises.push_back(model);
|
||||
noises.push_back(model);
|
||||
|
||||
vector<boost::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera)
|
||||
Ks.push_back(K);
|
||||
Ks.push_back(K);
|
||||
|
|
@ -199,7 +195,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
|
|||
views.push_back(x1);
|
||||
views.push_back(x2);
|
||||
|
||||
factor2->add(measurements, views, noises, Ks);
|
||||
factor2->add(measurements, views, Ks);
|
||||
|
||||
double actualError2 = factor2->error(values);
|
||||
|
||||
|
|
@ -241,14 +237,14 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
|
||||
SmartStereoProjectionParams smart_params;
|
||||
smart_params.triangulation.enableEPI = true;
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(smart_params));
|
||||
smartFactor1->add(measurements_l1, views, model, K2);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params));
|
||||
smartFactor1->add(measurements_l1, views, K2);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(smart_params));
|
||||
smartFactor2->add(measurements_l2, views, model, K2);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params));
|
||||
smartFactor2->add(measurements_l2, views, K2);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(smart_params));
|
||||
smartFactor3->add(measurements_l3, views, model, K2);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params));
|
||||
smartFactor3->add(measurements_l3, views, K2);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -383,14 +379,14 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
|
|||
SmartStereoProjectionParams params;
|
||||
params.setLinearizationMode(JACOBIAN_SVD);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor1->add(measurements_cam1, views, K);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor2->add(measurements_cam2, views, K);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor3->add(measurements_cam3, views, K);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -452,14 +448,14 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
|
|||
params.setLinearizationMode(JACOBIAN_SVD);
|
||||
params.setLandmarkDistanceThreshold(2);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor1->add(measurements_cam1, views, K);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor2->add(measurements_cam2, views, K);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor3->add(measurements_cam3, views, K);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -526,21 +522,21 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
params.setDynamicOutlierRejectionThreshold(1);
|
||||
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor1->add(measurements_cam1, views, K);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor2->add(measurements_cam2, views, K);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor3->add(measurements_cam3, views, K);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor4->add(measurements_cam4, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor4->add(measurements_cam4, views, K);
|
||||
|
||||
// same as factor 4, but dynamic outlier rejection is off
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor());
|
||||
smartFactor4b->add(measurements_cam4, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor(model));
|
||||
smartFactor4b->add(measurements_cam4, views, K);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
|
|
@ -739,14 +735,14 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
|
|||
SmartStereoProjectionParams params;
|
||||
params.setRankTolerance(10);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor1->add(measurements_cam1, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor1->add(measurements_cam1, views, K);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor2->add(measurements_cam2, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor2->add(measurements_cam2, views, K);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params));
|
||||
smartFactor3->add(measurements_cam3, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
|
||||
smartFactor3->add(measurements_cam3, views, K);
|
||||
|
||||
// Create graph to optimize
|
||||
NonlinearFactorGraph graph;
|
||||
|
|
@ -997,8 +993,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
|
|||
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark1);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor());
|
||||
smartFactorInstance->add(measurements_cam1, views, model, K);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor(model));
|
||||
smartFactorInstance->add(measurements_cam1, views, K);
|
||||
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
|
|
@ -1065,8 +1061,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
|||
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark1);
|
||||
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor());
|
||||
smartFactor->add(measurements_cam1, views, model, K2);
|
||||
SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor(model));
|
||||
smartFactor->add(measurements_cam1, views, K2);
|
||||
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
|
|
|
|||
Loading…
Reference in New Issue