diff --git a/gtsam_unstable/slam/TransformCalProjectionFactor.h b/gtsam_unstable/slam/TransformCalProjectionFactor.h new file mode 100644 index 000000000..ac796655e --- /dev/null +++ b/gtsam_unstable/slam/TransformCalProjectionFactor.h @@ -0,0 +1,173 @@ +/* ---------------------------------------------------------------------------- + + * 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 TransformCalProjectionFactor.h + * @brief Basic bearing factor from 2D measurement + * @author Chris Beall + * @author Richard Roberts + * @author Frank Dellaert + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. + * i.e. the main building block for visual SLAM. + * @addtogroup SLAM + */ + template + class TransformCalProjectionFactor: public NoiseModelFactor4 { + protected: + + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + + // verbosity handling for Cheirality Exceptions + bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) + bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) + + public: + + /// shorthand for base class type + typedef NoiseModelFactor4 Base; + + /// shorthand for this class + typedef TransformCalProjectionFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor + TransformCalProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} + + /** + * Constructor + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + */ + TransformCalProjectionFactor(const Point2& measured, const SharedNoiseModel& model, + Key poseKey, Key transformKey, Key pointKey, Key calibKey) : + Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured), + throwCheirality_(false), verboseCheirality_(false) {} + + /** + * Constructor with exception-handling flags + * TODO: Mark argument order standard (keys, measurement, parameters) + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation + * @param poseKey is the index of the camera + * @param pointKey is the index of the landmark + * @param K shared pointer to the constant calibration + * @param throwCheirality determines whether Cheirality exceptions are rethrown + * @param verboseCheirality determines whether exceptions are printed for Cheirality + */ + TransformCalProjectionFactor(const Point2& measured, const SharedNoiseModel& model, + Key poseKey, Key transformKey, Key pointKey, Key calibKey, + bool throwCheirality, bool verboseCheirality) : + Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured), + throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} + + /** Virtual destructor */ + virtual ~TransformCalProjectionFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "TransformCalProjectionFactor, z = "; + measured_.print(); + Base::print("", keyFormatter); + } + + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e + && Base::equals(p, tol) + && this->measured_.equals(e->measured_, tol); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, const CALIBRATION& K, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none) const { + try { + if(H1 || H2 || H3 || H4) { + gtsam::Matrix H0, H02; + PinholeCamera camera(pose.compose(transform, H0, H02), K); + Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_); + *H2 = *H1 * H02; + *H1 = *H1 * H0; + return reprojectionError.vector(); + } else { + PinholeCamera camera(pose.compose(transform), K); + Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_); + return reprojectionError.vector(); + } + } catch( CheiralityException& e) { + if (H1) *H1 = zeros(2,6); + if (H2) *H2 = zeros(2,6); + if (H3) *H3 = zeros(2,3); + if (H4) *H4 = zeros(2,CALIBRATION::Dim()); + if (verboseCheirality_) + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + if (throwCheirality_) + throw e; + } + return ones(2) * 2.0 * K.fx(); + } + + /** return the measurement */ + const Point2& measured() const { + return measured_; + } + + /** return verbosity */ + inline bool verboseCheirality() const { return verboseCheirality_; } + + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { return throwCheirality_; } + + private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(throwCheirality_); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); + } + }; +} // \ namespace gtsam diff --git a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m new file mode 100644 index 000000000..236327b05 --- /dev/null +++ b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m @@ -0,0 +1,189 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Read graph from file and perform GraphSLAM +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clear all; +clc; +import gtsam.* + +write_video = false; + +if(write_video) + videoObj = VideoWriter('test.avi'); + videoObj.Quality = 100; + videoObj.FrameRate = 2; + open(videoObj); +end + +%% generate some landmarks +nrPoints = 8; + landmarks = {Point3([20 15 1]'),... + Point3([22 7 -1]'),... + Point3([20 20 6]'),... + Point3([24 19 -4]'),... + Point3([26 17 -2]'),... + Point3([12 15 4]'),... + Point3([25 11 -6]'),... + Point3([23 10 4]')}; + +curvature = 5.0; +transformKey = 1000; +calibrationKey = 2000; + +fg = NonlinearFactorGraph; +initial = Values; + +%% intial landmarks and camera trajectory shifted in + y-direction +y_shift = Point3(0,1,0); + +% insert shifted points +for i=1:nrPoints + initial.insert(100+i,landmarks{i}.compose(y_shift)); +end + +figure(1); +cla +hold on; + +%% initial pose priors +pose_cov = noiseModel.Diagonal.Sigmas([1*pi/180; 1*pi/180; 1*pi/180; 0.1; 0.1; 0.1]); +fg.add(PriorFactorPose3(1, Pose3(),pose_cov)); +fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov)); + +%% Actual camera translation coincides with odometry, but -90deg Z-X rotation +camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift); +actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3()); +initial.insert(transformKey,camera_transform); + +trans_cov = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 20; 20; 20]); +fg.add(PriorFactorPose3(transformKey,camera_transform,trans_cov)); + + +%% insert poses +initial.insert(1, Pose3()); + + +move_forward = Pose3(Rot3(),Point3(1,0,0)); +move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(1,0,0)); +covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]); +z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]); + +%% calibration initialization +K = Cal3_S2(900,900,0,640,480); +K_corrupt = Cal3_S2(910,890,0,650,470); +initial.insert(2000, K_corrupt); + +K_cov = noiseModel.Diagonal.Sigmas([20; 20; 0.001; 20; 20]); +fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov)); + + +cheirality_exception_count = 0; + +isamParams = gtsam.ISAM2Params; +isamParams.setFactorization('QR'); +isam = ISAM2(isamParams); + +result = initial + +for i=1:20 + + if i > 1 + if i < 11 + initial.insert(i,result.at(i-1).compose(move_forward)); + fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); + else + initial.insert(i,result.at(i-1).compose(move_circle)); + fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); + end + + end + + % generate some camera measurements + cam_pose = initial.at(i).compose(actual_transform); +% gtsam.plotPose3(cam_pose); + cam = SimpleCamera(cam_pose,K); + i +% result + for j=1:nrPoints + % All landmarks seen in every frame + try + z = cam.project(landmarks{j}); + fg.add(TransformCalProjectionFactorCal3_S2(z, z_cov, i, transformKey, 100+j, calibrationKey)); + catch + cheirality_exception_count = cheirality_exception_count + 1; + end % end try/catch + end + + if i > 2 + disp('ISAM Update'); + isam.update(fg, initial); + result = isam.calculateEstimate(); + + %% reset + initial = Values; + fg = NonlinearFactorGraph; + end + + hold off; + + clf; + hold on; + + %% plot results + result_camera_transform = result.at(transformKey); + for j=1:i + gtsam.plotPose3(result.at(j),[],0.5); + gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5); + end + + xlabel('x (m)'); + ylabel('y (m)'); + + title(sprintf('Curvature %g deg, iteration %g', curvature, i)); + + axis([0 20 0 20 -10 10]); + view(-37,40); +% axis equal + + for l=101:100+nrPoints + plotPoint3(result.at(l),'g'); + end + + ty = result.at(transformKey).translation().y(); + fx = result.at(calibrationKey).fx(); + fy = result.at(calibrationKey).fy(); + text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty)); + text(1,5,3,sprintf('fx(900): %.0f',fx)); + text(1,5,1,sprintf('fy(900): %.0f',fy)); + + if(write_video) + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame) + else + pause(0.1); + end + + +end + +if(write_video) + close(videoObj); +end + +fprintf('Cheirality Exception count: %d\n', cheirality_exception_count); + +disp('Transform after optimization'); +result.at(transformKey) + +disp('Calibration after optimization'); +result.at(calibrationKey) + + + diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m new file mode 100644 index 000000000..410b7df0f --- /dev/null +++ b/matlab/unstable_examples/TransformProjectionFactorExample.m @@ -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 +% +% @brief Read graph from file and perform GraphSLAM +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clear all; +clc; +import gtsam.* + +%% generate some landmarks +nrPoints = 8; + landmarks = {Point3([20 15 1]'),... + Point3([22 7 1]'),... + Point3([20 20 6]'),... + Point3([24 19 4]'),... + Point3([26 17 2]'),... + Point3([12 15 4]'),... + Point3([25 11 6]'),... + Point3([23 10 4]')}; + +fg = NonlinearFactorGraph; +fg.add(NonlinearEqualityPose3(1, Pose3())); +initial = Values; + +%% intial landmarks and camera trajectory shifted in + y-direction +y_shift = Point3(0,1,0); + +% insert shifted points +for i=1:nrPoints + initial.insert(100+i,landmarks{i}.compose(y_shift)); +end + +figure(1); +cla +hold on; +plot3DPoints(initial); + +%% Actual camera translation coincides with odometry, but -90deg Z-X rotation +camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift); +actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3()); +initial.insert(1000,camera_transform); + +%% insert poses +initial.insert(1, Pose3()); + + +move_forward = Pose3(Rot3(),Point3(1,0,0)); +move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,0.2),Point3(1,0,0)); +covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]); +z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]); + +K = Cal3_S2(900,900,0,640,480); +cheirality_exception_count = 0; + +for i=1:20 + if i > 1 + if i < 11 + initial.insert(i,initial.at(i-1).compose(move_forward)); + fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); + else + initial.insert(i,initial.at(i-1).compose(move_circle)); + fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); + end + + end + + % generate some camera measurements + cam_pose = initial.at(i).compose(actual_transform); + gtsam.plotPose3(cam_pose); + cam = SimpleCamera(cam_pose,K); + i + for j=1:nrPoints + % All landmarks seen in every frame + try + z = cam.project(landmarks{j}); + fg.add(TransformProjectionFactorCal3_S2(z, z_cov, i, 1000, 100+j, K)); + catch + cheirality_exception_count = cheirality_exception_count + 1; + end % end try/catch + end + +end + +fprintf('Cheirality Exception count: %d\n', cheirality_exception_count); + +% plot3DTrajectory(initial, 'g-*'); + +%% camera plotting +for i=1:20 + gtsam.plotPose3(initial.at(i).compose(camera_transform)); +end + +xlabel('x (m)'); +ylabel('y (m)'); + +disp('Transform before optimization'); +initial.at(1000) + +params = LevenbergMarquardtParams; +params.setAbsoluteErrorTol(1e-15); +params.setRelativeErrorTol(1e-15); +params.setVerbosity('ERROR'); +params.setVerbosityLM('VERBOSE'); + +optimizer = LevenbergMarquardtOptimizer(fg, initial, params); +result = optimizer.optimizeSafely(); + +disp('Transform after optimization'); +result.at(1000) + +axis([0 25 0 25 0 10]); +axis equal +view(-37,40) + diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m new file mode 100644 index 000000000..169736f4b --- /dev/null +++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m @@ -0,0 +1,174 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Read graph from file and perform GraphSLAM +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clear all; +clc; +import gtsam.* + +write_video = true; + +if(write_video) + videoObj = VideoWriter('test.avi'); + videoObj.Quality = 100; + videoObj.FrameRate = 2; + open(videoObj); +end + +%% generate some landmarks +nrPoints = 8; + landmarks = {Point3([20 15 1]'),... + Point3([22 7 -1]'),... + Point3([20 20 6]'),... + Point3([24 19 -4]'),... + Point3([26 17 -2]'),... + Point3([12 15 4]'),... + Point3([25 11 -6]'),... + Point3([23 10 4]')}; + +fg = NonlinearFactorGraph; +pose_cov = noiseModel.Diagonal.Sigmas([1*pi/180; 1*pi/180; 1*pi/180; 0.1; 0.1; 0.1]); +fg.add(PriorFactorPose3(1, Pose3(),pose_cov)); +fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov)); + +curvature = 0.5; + +initial = Values; + +%% intial landmarks and camera trajectory shifted in + y-direction +y_shift = Point3(0,1,0); + +% insert shifted points +for i=1:nrPoints + initial.insert(100+i,landmarks{i}.compose(y_shift)); +end + +figure(1); +cla +hold on; + + +%% Actual camera translation coincides with odometry, but -90deg Z-X rotation +camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift); +actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3()); +initial.insert(1000,camera_transform); + +trans_cov = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 20; 20; 20]); +fg.add(PriorFactorPose3(1000,camera_transform,trans_cov)); + + +%% insert poses +initial.insert(1, Pose3()); + + +move_forward = Pose3(Rot3(),Point3(1,0,0)); +move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(1,0,0)); +covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]); +z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]); + +K = Cal3_S2(900,900,0,640,480); +cheirality_exception_count = 0; + +isamParams = gtsam.ISAM2Params; +isamParams.setFactorization('QR'); +isam = ISAM2(isamParams); + +result = initial + +for i=1:20 + + if i > 1 + if i < 11 + initial.insert(i,result.at(i-1).compose(move_forward)); + fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); + else + initial.insert(i,result.at(i-1).compose(move_circle)); + fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); + end + + end + + % generate some camera measurements + cam_pose = initial.at(i).compose(actual_transform); +% gtsam.plotPose3(cam_pose); + cam = SimpleCamera(cam_pose,K); + i +% result + for j=1:nrPoints + % All landmarks seen in every frame + try + z = cam.project(landmarks{j}); + fg.add(TransformProjectionFactorCal3_S2(z, z_cov, i, 1000, 100+j, K)); + catch + cheirality_exception_count = cheirality_exception_count + 1; + end % end try/catch + end + + if i > 2 + disp('ISAM Update'); + isam.update(fg, initial); + result = isam.calculateEstimate(); + + %% reset + initial = Values; + fg = NonlinearFactorGraph; + end + + hold off; + + clf; + hold on; + + %% plot results + result_camera_transform = result.at(1000); + for j=1:i + gtsam.plotPose3(result.at(j)); + gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5); + end + + xlabel('x (m)'); + ylabel('y (m)'); + + title(sprintf('Curvature %g deg, iteration %g', curvature, i)); + + axis([0 20 0 20 -10 10]); + view(-37,40); +% axis equal + + for l=101:100+nrPoints + plotPoint3(result.at(l),'g'); + end + + ty = result.at(1000).translation().y(); + text(5,5,5,sprintf('Y-Transform: %0.2g',ty)); + + if(write_video) + currFrame = getframe(gcf); + writeVideo(videoObj, currFrame) + else + pause(0.001); + end + + +end + +if(write_video) + close(videoObj); +end + +fprintf('Cheirality Exception count: %d\n', cheirality_exception_count); + + + +disp('Transform after optimization'); +result.at(1000) + + +