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

Transform + Calibration examples and factor

release/4.3a0
cbeall3 2014-07-01 16:33:23 -04:00
parent 5b2f4a2c3a
commit 8bc87e8f4f
4 changed files with 656 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

173
gtsam_unstable/slam/TransformCalProjectionFactor.h Normal file
View File

@ -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 <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/optional.hpp>
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 POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class TransformCalProjectionFactor: public NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> {
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<POSE, POSE, LANDMARK, CALIBRATION> Base;
/// shorthand for this class
typedef TransformCalProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> 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>(
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<const This*>(&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<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const {
try {
if(H1 || H2 || H3 || H4) {
gtsam::Matrix H0, H02;
PinholeCamera<CALIBRATION> 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<CALIBRATION> 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<class ARCHIVE>
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

189
matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m Normal file
View File

@ -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)

120
matlab/unstable_examples/TransformProjectionFactorExample.m Normal file
View File

@ -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)

174
matlab/unstable_examples/TransformProjectionFactorExampleISAM.m Normal file
View File

@ -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)