From 29ae7226bc364aaa8902f6fde6ba5789fb59c055 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 28 Jan 2014 12:19:25 -0500 Subject: [PATCH 01/90] Adding imuSimulator directory from old branch for new imu factors --- .../+imuSimulator/+lib/antisim.m | 7 + .../+imuSimulator/+lib/arrow3d.m | 140 +++++++++++++++++ .../+imuSimulator/+lib/getxyz.m | 14 ++ .../+imuSimulator/+lib/plot_trajectory.m | 19 +++ .../+imuSimulator/+lib/ref_frame_plot.m | 54 +++++++ .../+imuSimulator/+lib/rotatePoints.m | 82 ++++++++++ .../+imuSimulator/+lib/uth2rot.m | 14 ++ .../+imuSimulator/IMUComparison.m | 146 ++++++++++++++++++ .../+imuSimulator/IMUComparison_with_cov.m | 128 +++++++++++++++ .../+imuSimulator/calculateIMUMeas_coriolis.m | 14 ++ .../+imuSimulator/calculateIMUMeasurement.m | 26 ++++ .../+imuSimulator/integrateIMUTrajectory.m | 17 ++ .../integrateIMUTrajectory_bodyFrame.m | 26 ++++ .../integrateIMUTrajectory_navFrame.m | 21 +++ .../+imuSimulator/integrateTrajectory.m | 24 +++ .../+imuSimulator/test1onestep.m | 48 ++++++ .../+imuSimulator/test2constglobal.m | 34 ++++ .../+imuSimulator/test3constbody.m | 96 ++++++++++++ .../+imuSimulator/test4circle.m | 97 ++++++++++++ 19 files changed, 1007 insertions(+) create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/antisim.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/getxyz.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m create mode 100644 matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m create mode 100644 matlab/unstable_examples/+imuSimulator/IMUComparison.m create mode 100644 matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m create mode 100644 matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m create mode 100644 matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m create mode 100644 matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m create mode 100644 matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m create mode 100644 matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m create mode 100644 matlab/unstable_examples/+imuSimulator/integrateTrajectory.m create mode 100644 matlab/unstable_examples/+imuSimulator/test1onestep.m create mode 100644 matlab/unstable_examples/+imuSimulator/test2constglobal.m create mode 100644 matlab/unstable_examples/+imuSimulator/test3constbody.m create mode 100644 matlab/unstable_examples/+imuSimulator/test4circle.m diff --git a/matlab/unstable_examples/+imuSimulator/+lib/antisim.m b/matlab/unstable_examples/+imuSimulator/+lib/antisim.m new file mode 100644 index 000000000..6e9194567 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/antisim.m @@ -0,0 +1,7 @@ +function S = antisim(v) + +% costruisce la matrice antisimmetrica S (3x3) a partire dal vettore v +% Uso: S = antisim(v) + +S=[0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0]; + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m b/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m new file mode 100644 index 000000000..aef3a5f10 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m @@ -0,0 +1,140 @@ +function arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio, rhoRatio) + +% arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% Used to plot a single 3D arrow with a cylindrical stem and cone arrowhead +% pos = [X,Y,Z] - spatial location of the starting point of the arrow (end of stem) +% deltaValues = [QX,QY,QZ] - delta parameters denoting the magnitude of the arrow along the x,y,z-axes (relative to 'pos') +% colorCode - Color parameters as per the 'surf' command. For example, 'r', 'red', [1 0 0] are all examples of a red-colored arrow +% stemRatio - The ratio of the length of the stem in proportion to the arrowhead. For example, a call of: +% arrow3D([0,0,0], [100,0,0] , 'r', 0.82) will produce a red arrow of magnitude 100, with the arrowstem spanning a distance +% of 82 (note 0.82 ratio of length 100) while the arrowhead (cone) spans 18. +% rhoRatio - The ratio of the cylinder radius (0.05 is the default) +% value) +% +% Example: +% arrow3D([0,0,0], [4,3,7]); %---- arrow with default parameters +% axis equal; +% +% Author: Shawn Arseneau +% Created: September 14, 2006 +% Updated: September 18, 2006 +% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + if nargin<2 || nargin>5 + error('Incorrect number of inputs to arrow3D'); + end + if numel(pos)~=3 || numel(deltaValues)~=3 + error('pos and/or deltaValues is incorrect dimensions (should be three)'); + end + if nargin<3 + colorCode = 'interp'; + end + if nargin<4 + stemRatio = 0.75; + end + if nargin<5 + rhoRatio = 0.05; + end + + X = pos(1); %---- with this notation, there is no need to transpose if the user has chosen a row vs col vector + Y = pos(2); + Z = pos(3); + + [sphi, stheta, srho] = cart2sph(deltaValues(1), deltaValues(2), deltaValues(3)); + + %******************************************* CYLINDER == STEM ********************************************* + cylinderRadius = srho*rhoRatio; + cylinderLength = srho*stemRatio; + [CX,CY,CZ] = cylinder(cylinderRadius); + CZ = CZ.*cylinderLength; %---- lengthen + + %----- ROTATE CYLINDER + [row, col] = size(CX); %---- initial rotation to coincide with X-axis + + newEll = rotatePoints([0 0 -1], [CX(:), CY(:), CZ(:)]); + CX = reshape(newEll(:,1), row, col); + CY = reshape(newEll(:,2), row, col); + CZ = reshape(newEll(:,3), row, col); + + [row, col] = size(CX); + newEll = rotatePoints(deltaValues, [CX(:), CY(:), CZ(:)]); + stemX = reshape(newEll(:,1), row, col); + stemY = reshape(newEll(:,2), row, col); + stemZ = reshape(newEll(:,3), row, col); + + %----- TRANSLATE CYLINDER + stemX = stemX + X; + stemY = stemY + Y; + stemZ = stemZ + Z; + + %******************************************* CONE == ARROWHEAD ********************************************* + coneLength = srho*(1-stemRatio); + coneRadius = cylinderRadius*1.5; + incr = 4; %---- Steps of cone increments + coneincr = coneRadius/incr; + [coneX, coneY, coneZ] = cylinder(cylinderRadius*2:-coneincr:0); %---------- CONE + coneZ = coneZ.*coneLength; + + %----- ROTATE CONE + [row, col] = size(coneX); + newEll = rotatePoints([0 0 -1], [coneX(:), coneY(:), coneZ(:)]); + coneX = reshape(newEll(:,1), row, col); + coneY = reshape(newEll(:,2), row, col); + coneZ = reshape(newEll(:,3), row, col); + + newEll = rotatePoints(deltaValues, [coneX(:), coneY(:), coneZ(:)]); + headX = reshape(newEll(:,1), row, col); + headY = reshape(newEll(:,2), row, col); + headZ = reshape(newEll(:,3), row, col); + + %---- TRANSLATE CONE + V = [0, 0, srho*stemRatio]; %---- centerline for cylinder: the multiplier is to set the cone 'on the rim' of the cylinder + Vp = rotatePoints([0 0 -1], V); + Vp = rotatePoints(deltaValues, Vp); + headX = headX + Vp(1) + X; + headY = headY + Vp(2) + Y; + headZ = headZ + Vp(3) + Z; + %************************************************************************************************************ + hStem = surf(stemX, stemY, stemZ, 'FaceColor', colorCode, 'EdgeColor', 'none'); + hold on; + hHead = surf(headX, headY, headZ, 'FaceColor', colorCode, 'EdgeColor', 'none'); + + if nargout==1 + arrowHandle = [hStem, hHead]; + end + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m b/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m new file mode 100644 index 000000000..d2c70db3a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m @@ -0,0 +1,14 @@ +function [c] = getxyz(poses, j) +% The function extract the Cartesian variables from pose (pose.p = positions, +% pose.R = rotations). In particular, if there are T poses, +% - getxyz(pose, 1) estracts the vector x \in R^T, +% - getxyz(pose, 2) estracts the vector y \in R^T, +% - getxyz(pose, 3) estracts the vector z \in R^T. + +L = length(poses); +c = []; +for i=1:L % for each pose + c = [c poses(i).p(j)]; +end + +c = c(:); % column vector \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m b/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m new file mode 100644 index 000000000..1bababd33 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m @@ -0,0 +1,19 @@ +function [] = plot_trajectory(pose, step, color, plot_name,a,b,c) +% Plot a collection of poses: positions are connected by a solid line +% of color "color" and it is shown a ref frame for each pose. +% Plot_name defines the name of the created figure. + +x = getxyz(pose,1); +y = getxyz(pose,2); +z = getxyz(pose,3); +plot3(x,y,z,color) +n = length(x)-1; +hold on +for i=1:step:n+1 + ref_frame_plot(pose(i).R,pose(i).p,a,b,c) +end +title(plot_name); +xlabel('x') +ylabel('y') +zlabel('z') +view(3) \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m b/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m new file mode 100644 index 000000000..9c73d1c69 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m @@ -0,0 +1,54 @@ +function ref_frame_plot(Ref,OriginRef,rhoRatio,stemRatio,axsize) +% ref_frame plot a 3D representation of a reference frame +% given by three unit vectors +% +% ref_frame(Ref,DimSpace,OriginRef) +% Ref is a 3 x 3 orthogonal matrix representing the unit vectors +% of the reference frame to be drawn +% DimSpace is a 3 x 2 matrix with min an max dimensions of the space +% [xmin xmax; ymin ymax; zmin zmax] +% default value = [-1,5 +1.5] for all dimensions +% OriginRef is the reference frame origin point +% default value = [0 0 0]' + +% Copright (C) Basilio Bona 2007 + +n=nargin; +if n == 1 + OriginRef=[0 0 0]'; + colorcode=['r','g','b']; + rhoRatio=0.05; + stemRatio=0.75; + axsize=1; +end +if n == 2 + colorcode=['r','g','b']; + rhoRatio=0.05; + stemRatio=0.75; + axsize=1; +end +if n == 3 + colorcode=['r','g','b']; + stemRatio=0.75; + axsize=1; +end + +if n == 4 + colorcode=['r','g','b']; + axsize=1; +end + +if n == 5 + colorcode=['r','g','b']; +end + + +% xproj=DimSpace(1,2); yproj=DimSpace(2,2); zproj=DimSpace(3,1); + +%hold off; +arrow3d(OriginRef, axsize*Ref(:,1), colorcode(1), stemRatio, rhoRatio); +arrow3d(OriginRef, axsize*Ref(:,2), colorcode(2), stemRatio, rhoRatio); +arrow3d(OriginRef, axsize*Ref(:,3), colorcode(3), stemRatio, rhoRatio); +axis equal; xlabel('X'); ylabel('Y'); zlabel('Z'); +% lighting phong; +% camlight left; diff --git a/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m b/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m new file mode 100644 index 000000000..4380d9d5d --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m @@ -0,0 +1,82 @@ +function rotatedData = rotatePoints(alignmentVector, originalData) + +% rotatedData = rotatePoints(alignmentVector, originalData) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% Rotate the 'originalData' in the form of Nx2 or Nx3 about the origin by aligning the x-axis with the alignment vector +% +% Rdata = rotatePoints([1,2,-1], [Xpts(:), Ypts(:), Zpts(:)]) - rotate the (X,Y,Z)pts in 3D with respect to the vector [1,2,-1] +% +% Rotating using spherical components can be done by first converting using [dX,dY,dZ] = cart2sph(theta, phi, rho); alignmentVector = [dX,dY,dZ]; +% +% Example: +% %% Rotate the point [3,4,-7] with respect to the following: +% %%%% Original associated vector is always [1,0,0] +% %%%% Calculate the appropriate rotation requested with respect to the x-axis. For example, if only a rotation about the z-axis is +% %%%% sought, alignmentVector = [2,1,0] %% Note that the z-component is zero +% rotData = rotatePoints(alignmentVector, [3,4,-7]); +% +% Author: Shawn Arseneau +% Created: Feb.2, 2006 +% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + alignmentDim = numel(alignmentVector); + DOF = size(originalData,2); %---- DOF = Degrees of Freedom (i.e. 2 for two dimensional and 3 for three dimensional data) + + if alignmentDim~=DOF + error('Alignment vector does not agree with originalData dimensions'); + end + if DOF<2 || DOF>3 + error('rotatePoints only does rotation in two or three dimensions'); + end + + + if DOF==2 % 2D rotation... + [rad_theta, rho] = cart2pol(alignmentVector(1), alignmentVector(2)); + deg_theta = -1 * rad_theta * (180/pi); + ctheta = cosd(deg_theta); stheta = sind(deg_theta); + + Rmatrix = [ctheta, -1.*stheta;... + stheta, ctheta]; + rotatedData = originalData*Rmatrix; + + else % 3D rotation... + [rad_theta, rad_phi, rho] = cart2sph(alignmentVector(1), alignmentVector(2), alignmentVector(3)); + rad_theta = rad_theta * -1; + deg_theta = rad_theta * (180/pi); + deg_phi = rad_phi * (180/pi); + ctheta = cosd(deg_theta); stheta = sind(deg_theta); + Rz = [ctheta, -1.*stheta, 0;... + stheta, ctheta, 0;... + 0, 0, 1]; %% First rotate as per theta around the Z axis + rotatedData = originalData*Rz; + + [rotX, rotY, rotZ] = sph2cart(-1* (rad_theta+(pi/2)), 0, 1); %% Second rotation corresponding to phi + rotationAxis = [rotX, rotY, rotZ]; + u = rotationAxis(:)/norm(rotationAxis); %% Code extract from rotate.m from MATLAB + cosPhi = cosd(deg_phi); + sinPhi = sind(deg_phi); + invCosPhi = 1 - cosPhi; + x = u(1); + y = u(2); + z = u(3); + Rmatrix = [cosPhi+x^2*invCosPhi x*y*invCosPhi-z*sinPhi x*z*invCosPhi+y*sinPhi; ... + x*y*invCosPhi+z*sinPhi cosPhi+y^2*invCosPhi y*z*invCosPhi-x*sinPhi; ... + x*z*invCosPhi-y*sinPhi y*z*invCosPhi+x*sinPhi cosPhi+z^2*invCosPhi]'; + + rotatedData = rotatedData*Rmatrix; + end + + + + + + + + + + + + + + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m b/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m new file mode 100644 index 000000000..6735ad6e5 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m @@ -0,0 +1,14 @@ +function R = uth2rot(u,teta) + +% Uso: R = uth2rot(u,teta) +% +% calcola la matrice R +% a partire da asse u ed angolo di rotazione teta (in rad) + +S=antisim(u); +t=teta; + +n=norm(u); + +R = eye(3) + sin(t)/n*S + (1-cos(t))/n^2*S^2; + diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m new file mode 100644 index 000000000..68e20bb25 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m @@ -0,0 +1,146 @@ +import gtsam.*; + +deltaT = 0.001; +summarizedDeltaT = 0.1; +timeElapsed = 1; +times = 0:deltaT:timeElapsed; + +omega = [0;0;2*pi]; +velocity = [1;0;0]; + +summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ... + gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ... + 1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3)); + +%% Set initial conditions for the true trajectory and for the estimates +% (one estimate is obtained by integrating in the body frame, the other +% by integrating in the navigation frame) +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; +% Initial state estimate (integrating in navigation frame) +currentPoseGlobalIMUnav = currentPoseGlobal; +currentVelocityGlobalIMUnav = currentVelocityGlobal; +% Initial state estimate (integrating in the body frame) +currentPoseGlobalIMUbody = currentPoseGlobal; +currentVelocityGlobalIMUbody = currentVelocityGlobal; + +%% Prepare data structures for actual trajectory and estimates +% Actual trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +% Trajectory estimate (integrated in the navigation frame) +positionsIMUnav = zeros(3, length(times)+1); +positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUnav(1).p = positionsIMUnav(:,1); +posesIMUnav(1).R = poses(1).R; + +% Trajectory estimate (integrated in the body frame) +positionsIMUbody = zeros(3, length(times)+1); +positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUbody(1).p = positionsIMUbody(:,1); +posesIMUbody(1).R = poses(1).R; + +%% Solver object +isamParams = ISAM2Params; +isamParams.setRelinearizeSkip(1); +isam = gtsam.ISAM2(isamParams); + +initialValues = Values; +initialValues.insert(symbol('x',0), currentPoseGlobal); +initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); +initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); +initialFactors = NonlinearFactorGraph; +initialFactors.add(PriorFactorPose3(symbol('x',0), ... + currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0))); +initialFactors.add(PriorFactorLieVector(symbol('v',0), ... + LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0))); +initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... + imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0))); + +%% Main loop +i = 2; +lastSummaryTime = 0; +lastSummaryIndex = 0; +currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); +for t = times + %% Create the actual trajectory, using the velocities and + % accelerations in the inertial frame to compute the positions + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + %% Simulate IMU measurements, considering Coriolis effect + % (in this simple example we neglect gravity and there are no other forces acting on the body) + acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... + omega, omega, velocity, velocity, deltaT); + + %% Accumulate preintegrated measurement + currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT); + + %% Update solver + if t - lastSummaryTime >= summarizedDeltaT + % Create IMU factor + initialFactors.add(ImuFactor( ... + symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ... + symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ... + symbol('b',0), currentSummarizedMeasurement, [0;0;1], [0;0;0], ... + noiseModel.Isotropic.Sigma(9, 1e-6))); + + % Predict movement in a straight line (bad initialization) + if lastSummaryIndex > 0 + initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ... + .compose(Pose3(Rot3, Point3( velocity * t - lastSummaryTime) )); + initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); + else + initialPose = Pose3; + initialVel = LieVector(velocity); + end + initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); + initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); + + % Update solver + isam.update(initialFactors, initialValues); + initialFactors = NonlinearFactorGraph; + initialValues = Values; + + lastSummaryIndex = lastSummaryIndex + 1; + lastSummaryTime = t; + currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); + end + + %% Integrate in the body frame + [ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ... + currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity); + + %% Integrate in the navigation frame + [ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ... + currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT); + + %% Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector; + positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector; + % - + poses(i).p = positions(:,i); + posesIMUbody(i).p = positionsIMUbody(:,i); + posesIMUnav(i).p = positionsIMUnav(:,i); + % - + poses(i).R = currentPoseGlobal.rotation.matrix; + posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix; + posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix; + i = i + 1; +end + +figure(1) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r'); +plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k'); +plot3DTrajectory(isam.calculateEstimate, 'g-'); +axis equal; +legend('true trajectory', 'traj integrated in body', 'traj integrated in nav') + + diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m new file mode 100644 index 000000000..c589bea32 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m @@ -0,0 +1,128 @@ +close all +clc + +import gtsam.*; + +deltaT = 0.001; +summarizedDeltaT = 0.1; +timeElapsed = 1; +times = 0:deltaT:timeElapsed; + +omega = [0;0;2*pi]; +velocity = [1;0;0]; + +g = [0;0;0]; +cor_v = [0;0;0]; + +summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ... + gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ... + 1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3)); + +%% Set initial conditions for the true trajectory and for the estimates +% (one estimate is obtained by integrating in the body frame, the other +% by integrating in the navigation frame) +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; + +%% Prepare data structures for actual trajectory and estimates +% Actual trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +%% Solver object +isamParams = ISAM2Params; +isamParams.setRelinearizeSkip(1); +isam = gtsam.ISAM2(isamParams); + +sigma_init_x = 1.0; +sigma_init_v = 1.0; +sigma_init_b = 1.0; + +initialValues = Values; +initialValues.insert(symbol('x',0), currentPoseGlobal); +initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); +initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); +initialFactors = NonlinearFactorGraph; +% Prior on initial pose +initialFactors.add(PriorFactorPose3(symbol('x',0), ... + currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x))); +% Prior on initial velocity +initialFactors.add(PriorFactorLieVector(symbol('v',0), ... + LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v))); +% Prior on initial bias +initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... + imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b))); + +%% Main loop +i = 2; +lastSummaryTime = 0; +lastSummaryIndex = 0; +currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); +for t = times + %% Create the ground truth trajectory, using the velocities and accelerations in the inertial frame to compute the positions + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + %% Simulate IMU measurements, considering Coriolis effect + % (in this simple example we neglect gravity and there are no other forces acting on the body) + acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... + omega, omega, velocity, velocity, deltaT); + + %% Accumulate preintegrated measurement + currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT); + + %% Update solver + if t - lastSummaryTime >= summarizedDeltaT + + % Create IMU factor + initialFactors.add(ImuFactor( ... + symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ... + symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ... + symbol('b',0), currentSummarizedMeasurement, g, cor_v, ... + noiseModel.Isotropic.Sigma(9, 1e-6))); + + % Predict movement in a straight line (bad initialization) + if lastSummaryIndex > 0 + initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ... + .compose(Pose3(Rot3, Point3( velocity * t - lastSummaryTime) )); + initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); + else + initialPose = Pose3; + initialVel = LieVector(velocity); + end + initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); + initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); + + key_pose = symbol('x',lastSummaryIndex+1); + + % Update solver + isam.update(initialFactors, initialValues); + initialFactors = NonlinearFactorGraph; + initialValues = Values; + + isam.calculateEstimate(key_pose); + M = isam.marginalCovariance(key_pose); + + lastSummaryIndex = lastSummaryIndex + 1; + lastSummaryTime = t; + currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); + end + + %% Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + i = i + 1; +end + +figure(1) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot3DTrajectory(isam.calculateEstimate, 'g-'); + + +axis equal; +legend('true trajectory', 'traj integrated in body', 'traj integrated in nav') + + diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m new file mode 100644 index 000000000..c86e40a21 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m @@ -0,0 +1,14 @@ +function [ acc_omega ] = calculateIMUMeas_coriolis(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT) + +import gtsam.*; + +% gyro measured rotation rate +measuredOmega = omega1Body; + +% Acceleration measurement (in this simple toy example no other forces +% act on the body and the only acceleration is the centripetal Coriolis acceleration) +measuredAcc = Point3(cross(omega1Body, velocity1Body)).vector; +acc_omega = [ measuredAcc; measuredOmega ]; + +end + diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m new file mode 100644 index 000000000..534b9365e --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m @@ -0,0 +1,26 @@ +function [ acc_omega ] = calculateIMUMeasurement(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT, imuFrame) +%CALCULATEIMUMEASUREMENT Calculate measured body frame acceleration in +%frame 1 and measured angular rates in frame 1. + +import gtsam.*; + +% Calculate gyro measured rotation rate by transforming body rotation rate +% into the IMU frame. +measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body)).vector; + +% Transform both velocities into IMU frame, accounting for the velocity +% induced by rigid body rotation on a lever arm (Coriolis effect). +velocity1inertial = imuFrame.rotation.unrotate( ... + Point3(velocity1Body + cross(omega1Body, imuFrame.translation.vector))).vector; + +imu2in1 = Rot3.Expmap(measuredOmega * deltaT); +velocity2inertial = imu2in1.rotate(imuFrame.rotation.unrotate( ... + Point3(velocity2Body + cross(omega2Body, imuFrame.translation.vector)))).vector; + +% Acceleration in IMU frame +measuredAcc = (velocity2inertial - velocity1inertial) / deltaT; + +acc_omega = [ measuredAcc; measuredOmega ]; + +end + diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m new file mode 100644 index 000000000..3f72f1215 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m @@ -0,0 +1,17 @@ +function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory( ... + initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT) +%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement + +import gtsam.*; + +imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT); +accelGlobal = initialPoseGlobal.rotation().rotate(Point3(acc_omegaIMU(1:3))).vector; + +finalPosition = Point3(initialPoseGlobal.translation.vector ... + + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT); +finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT; +finalRotation = initialPoseGlobal.rotation.compose(imu2in1); +finalPose = Pose3(finalRotation, finalPosition); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m new file mode 100644 index 000000000..50b223060 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m @@ -0,0 +1,26 @@ +function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_bodyFrame( ... + initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT, velocity1Body) + +% Before integrating in the body frame we need to compensate for the Coriolis +% effect +acc_body = acc_omegaIMU(1:3) - Point3(cross(acc_omegaIMU(4:6), velocity1Body)).vector; +% after compensating for coriolis this will be essentially zero +% since we are moving at constant body velocity + +import gtsam.*; +%% Integrate in the body frame +% Integrate rotations +imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT); +% Integrate positions +finalPositionBody = velocity1Body * deltaT + 0.5 * acc_body * deltaT * deltaT; +finalVelocityBody = velocity1Body + acc_body * deltaT; + +%% Express the integrated quantities in the global frame +finalVelocityGlobal = initialVelocityGlobal + (initialPoseGlobal.rotation().rotate(Point3(finalVelocityBody)).vector() ); +finalPosition = initialPoseGlobal.translation().vector() + initialPoseGlobal.rotation().rotate( Point3(finalPositionBody)).vector() ; +finalRotation = initialPoseGlobal.rotation.compose(imu2in1); +% Include position and rotation in a pose +finalPose = Pose3(finalRotation, Point3(finalPosition) ); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m new file mode 100644 index 000000000..b919520ac --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m @@ -0,0 +1,21 @@ +function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_navFrame( ... + initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT) +%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement + +import gtsam.*; +% Integrate rotations +imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT); +finalRotation = initialPoseGlobal.rotation.compose(imu2in1); + +intermediateRotation = initialPoseGlobal.rotation.compose( Rot3.Expmap(acc_omegaIMU(4:6) * deltaT/2 )); +% Integrate positions (equation (1) in Lupton) +accelGlobal = intermediateRotation.rotate(Point3(acc_omegaIMU(1:3))).vector; +finalPosition = Point3(initialPoseGlobal.translation.vector ... + + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT); +finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT; + +% Include position and rotation in a pose +finalPose = Pose3(finalRotation, finalPosition); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m new file mode 100644 index 000000000..e51b622b0 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m @@ -0,0 +1,24 @@ +function [ finalPose, finalVelocityGlobal ] = integrateTrajectory( ... + initialPose, omega1Body, velocity1Body, velocity2Body, deltaT) +%INTEGRATETRAJECTORY Integrate one trajectory step + +import gtsam.*; +% Rotation: R^1_2 +body2in1 = Rot3.Expmap(omega1Body * deltaT); +% Velocity 2 in frame 1: v^1_2 = R^1_2 v^2_2 +velocity2inertial = body2in1.rotate(Point3(velocity2Body)).vector; +% Acceleration: a^1 = (v^1_2 - v^1_1)/dt +accelBody1 = (velocity2inertial - velocity1Body) / deltaT; + +% Velocity 1 in frame W: v^W_1 = R^W_1 v^1_1 +initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body)).vector; +% Acceleration in frame W: a^W = R^W_1 a^1 +accelGlobal = initialPose.rotation().rotate(Point3(accelBody1)).vector; + +finalPosition = Point3(initialPose.translation.vector + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT); +finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT; +finalRotation = initialPose.rotation.compose(body2in1); +finalPose = Pose3(finalRotation, finalPosition); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/test1onestep.m b/matlab/unstable_examples/+imuSimulator/test1onestep.m new file mode 100644 index 000000000..883569849 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test1onestep.m @@ -0,0 +1,48 @@ +import gtsam.*; + +deltaT = 0.01; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant global velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant global velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; +R = Rot3.Expmap(omega * deltaT); + +velocity2body = R.unrotate(Point3(velocity)).vector; +acc_omegaExpected = [-0.01; 0; 0; 0; 0; 0.1]; +acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity2body, deltaT, Pose3(Rot3, Point3([1;0;0]))); +disp('IMU measurement discrepancy:'); +disp(acc_omegaActual - acc_omegaExpected); + +initialPose = Pose3; +finalPoseExpected = Pose3(Rot3.Expmap(omega * deltaT), Point3(velocity * deltaT)); +finalVelocityExpected = velocity; +[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity2body, deltaT); +disp('Final pose discrepancy:'); +disp(finalPoseExpected.between(finalPoseActual).matrix); +disp('Final velocity discrepancy:'); +disp(finalVelocityActual - finalVelocityExpected); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant body velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant body velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; + +acc_omegaExpected = [-0.01; 0.1; 0; 0; 0; 0.1]; +acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity, deltaT, Pose3(Rot3, Point3([1;0;0]))); +disp('IMU measurement discrepancy:'); +disp(acc_omegaActual - acc_omegaExpected); + +initialPose = Pose3; +initialVelocity = velocity; +finalPoseExpected = Pose3.Expmap([ omega; velocity ] * deltaT); +finalVelocityExpected = finalPoseExpected.rotation.rotate(Point3(velocity)).vector; +[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity, deltaT); +disp('Final pose discrepancy:'); +disp(finalPoseExpected.between(finalPoseActual).matrix); +disp('Final velocity discrepancy:'); +disp(finalVelocityActual - finalVelocityExpected); \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/test2constglobal.m b/matlab/unstable_examples/+imuSimulator/test2constglobal.m new file mode 100644 index 000000000..19956d08a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test2constglobal.m @@ -0,0 +1,34 @@ +import gtsam.*; + +deltaT = 0.01; +timeElapsed = 1000; + +times = 0:deltaT:timeElapsed; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant global velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant global velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; + +% Initial state +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; + +% Positions +positions = zeros(3, length(times)+1); + +i = 2; +for t = times + velocity1body = currentPoseGlobal.rotation.unrotate(Point3(currentVelocityGlobal)).vector; + R = Rot3.Expmap(omega * deltaT); + velocity2body = currentPoseGlobal.rotation.compose(R).unrotate(Point3(currentVelocityGlobal)).vector; + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory(currentPoseGlobal, omega, velocity1body, velocity2body, deltaT); + + positions(:,i) = currentPoseGlobal.translation.vector; + i = i + 1; +end + +figure; +plot(positions(1,:), positions(2,:), '.-'); diff --git a/matlab/unstable_examples/+imuSimulator/test3constbody.m b/matlab/unstable_examples/+imuSimulator/test3constbody.m new file mode 100644 index 000000000..b3ee2edfc --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test3constbody.m @@ -0,0 +1,96 @@ +clc +clear all +close all + +import gtsam.*; + +addpath(genpath('./Libraries')) + +deltaT = 0.01; +timeElapsed = 25; + +times = 0:deltaT:timeElapsed; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant body velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant body velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; +RIMUinBody = Rot3.Rz(-pi/2); +% RIMUinBody = eye(3) +IMUinBody = Pose3(RIMUinBody, Point3([1;0;0])); + +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; +% Initial state (IMU) +currentPoseGlobalIMU = Pose3; %currentPoseGlobal.compose(IMUinBody); +%currentVelocityGlobalIMU = IMUinBody.rotation.unrotate(Point3(velocity)).vector; % no Coriolis here? +currentVelocityGlobalIMU = IMUinBody.rotation.unrotate( ... + Point3(velocity + cross(omega, IMUinBody.translation.vector))).vector; + + +% Positions +% body trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +% Expected IMU trajectory (from body trajectory and lever arm) +positionsIMUe = zeros(3, length(times)+1); +positionsIMUe(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector; +posesIMUe(1).p = positionsIMUe(:,1); +posesIMUe(1).R = poses(1).R * IMUinBody.rotation.matrix; + +% Integrated IMU trajectory (from IMU measurements) +positionsIMU = zeros(3, length(times)+1); +positionsIMU(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector; +posesIMU(1).p = positionsIMU(:,1); +posesIMU(1).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix; + +i = 2; +for t = times + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + acc_omega = imuSimulator.calculateIMUMeasurement( ... + omega, omega, velocity, velocity, deltaT, IMUinBody); + + [ currentPoseGlobalIMU, currentVelocityGlobalIMU ] = imuSimulator.integrateIMUTrajectory( ... + currentPoseGlobalIMU, currentVelocityGlobalIMU, acc_omega, deltaT); + + % Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + positionsIMUe(:,i) = currentPoseGlobal.translation.vector + currentPoseGlobal.rotation.matrix * IMUinBody.translation.vector; + positionsIMU(:,i) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector; + + poses(i).p = positions(:,i); + posesIMUe(i).p = positionsIMUe(:,i); + posesIMU(i).p = positionsIMU(:,i); + + poses(i).R = currentPoseGlobal.rotation.matrix; + posesIMUe(i).R = poses(i).R * IMUinBody.rotation.matrix; + posesIMU(i).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix; + i = i + 1; +end + + +figure(1) +plot_trajectory(poses, 50, '-k', 'body trajectory',0.1,0.75,1) +hold on +plot_trajectory(posesIMU, 50, '-r', 'imu trajectory',0.1,0.75,1) + +figure(2) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot(positionsIMU(1,:), positionsIMU(2,:), '-r'); +plot(positionsIMUe(1,:), positionsIMUe(2,:), ':k'); +axis equal; + +disp('Mismatch between final integrated IMU position and expected IMU position') +disp(norm(posesIMUe(end).p - posesIMU(end).p)) +disp('Mismatch between final integrated IMU orientation and expected IMU orientation') +disp(norm(posesIMUe(end).R - posesIMU(end).R)) + diff --git a/matlab/unstable_examples/+imuSimulator/test4circle.m b/matlab/unstable_examples/+imuSimulator/test4circle.m new file mode 100644 index 000000000..22ee175dd --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test4circle.m @@ -0,0 +1,97 @@ +clc +clear all +close all + +import gtsam.*; + +deltaT = 0.001; +timeElapsed = 1; +times = 0:deltaT:timeElapsed; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('--------------------------------------------------------'); +disp('Integration in body frame VS integration in navigation frame'); +disp('TOY EXAMPLE:'); +disp('- Body moves along a circular trajectory with constant rotation rate -omega- and constant -velocity- (in the body frame)'); +disp('- We compare two integration schemes: integating in the navigation frame (similar to Lupton approach) VS integrating in the body frame') +disp('- Navigation frame is assumed inertial for simplicity') +omega = [0;0;2*pi]; +velocity = [1;0;0]; + +%% Set initial conditions for the true trajectory and for the estimates +% (one estimate is obtained by integrating in the body frame, the other +% by integrating in the navigation frame) +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; +% Initial state estimate (integrating in navigation frame) +currentPoseGlobalIMUnav = currentPoseGlobal; +currentVelocityGlobalIMUnav = currentVelocityGlobal; +% Initial state estimate (integrating in the body frame) +currentPoseGlobalIMUbody = currentPoseGlobal; +currentVelocityGlobalIMUbody = currentVelocityGlobal; + +%% Prepare data structures for actual trajectory and estimates +% Actual trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +% Trajectory estimate (integrated in the navigation frame) +positionsIMUnav = zeros(3, length(times)+1); +positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUnav(1).p = positionsIMUnav(:,1); +posesIMUnav(1).R = poses(1).R; + +% Trajectory estimate (integrated in the body frame) +positionsIMUbody = zeros(3, length(times)+1); +positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUbody(1).p = positionsIMUbody(:,1); +posesIMUbody(1).R = poses(1).R; + +%% Main loop +i = 2; +for t = times + %% Create the actual trajectory, using the velocities and + % accelerations in the inertial frame to compute the positions + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + %% Simulate IMU measurements, considering Coriolis effect + % (in this simple example we neglect gravity and there are no other forces acting on the body) + acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... + omega, omega, velocity, velocity, deltaT); + + %% Integrate in the body frame + [ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ... + currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity); + + %% Integrate in the navigation frame + [ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ... + currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT); + + %% Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector; + positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector; + % - + poses(i).p = positions(:,i); + posesIMUbody(i).p = positionsIMUbody(:,i); + posesIMUnav(i).p = positionsIMUnav(:,i); + % - + poses(i).R = currentPoseGlobal.rotation.matrix; + posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix; + posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix; + i = i + 1; +end + +figure(1) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r'); +plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k'); +axis equal; +legend('true trajectory', 'traj integrated in body', 'traj integrated in nav') + + From 7049e83593b4c6c4abe9a88fadad1f2de55f2eb0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 29 Jan 2014 00:38:41 -0500 Subject: [PATCH 02/90] Just reading over both factors, whitespace changes only --- gtsam/navigation/.gitignore | 1 + gtsam/navigation/CombinedImuFactor.h | 3 +-- gtsam/navigation/ImuFactor.h | 19 ++++++++++--------- 3 files changed, 12 insertions(+), 11 deletions(-) create mode 100644 gtsam/navigation/.gitignore diff --git a/gtsam/navigation/.gitignore b/gtsam/navigation/.gitignore new file mode 100644 index 000000000..a32e68b1f --- /dev/null +++ b/gtsam/navigation/.gitignore @@ -0,0 +1 @@ +/*.h~ diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index f7e3cfec8..c807fd356 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -274,7 +274,6 @@ namespace gtsam { // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - // deltaPij += deltaVij * deltaT; deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; @@ -347,6 +346,7 @@ namespace gtsam { #else typedef boost::shared_ptr shared_ptr; #endif + /** Default constructor - only use for serialization */ CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} @@ -555,7 +555,6 @@ namespace gtsam { Matrix3::Zero(), Matrix3::Identity(); } - // Evaluate residual error, according to [3] /* ---------------------------------------------------------------------------------------------------- */ const Vector3 fp = diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index bd8a0f80b..7438541d7 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -33,7 +33,8 @@ namespace gtsam { /** * * @addtogroup SLAM - * * REFERENCES: + * + * REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. @@ -87,7 +88,7 @@ namespace gtsam { imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i) + Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) double deltaTij; ///< Time interval from i to j @@ -170,18 +171,14 @@ namespace gtsam { // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame if(body_P_sensor){ Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); // linear acceleration vector in the body frame } const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr // Update Jacobians @@ -190,12 +187,13 @@ namespace gtsam { // delPdelBiasOmega += delVdelBiasOmega * deltaT; delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() - * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + delVdelBiasAcc += -deltaRij.matrix() * deltaT; delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; - // Update preintegrated mesurements covariance + // Update preintegrated measurements covariance /* ----------------------------------------------------------------------------------------------------------------------- */ Matrix3 Z_3x3 = Matrix3::Zero(); Matrix3 I_3x3 = Matrix3::Identity(); @@ -424,6 +422,7 @@ namespace gtsam { // dfR/dPi Matrix3::Zero(); } + if(H2) { H2->resize(9,3); (*H2) << @@ -437,6 +436,7 @@ namespace gtsam { Matrix3::Zero(); } + if(H3) { H3->resize(9,6); @@ -448,6 +448,7 @@ namespace gtsam { // dfR/dPosej Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); } + if(H4) { H4->resize(9,3); (*H4) << @@ -458,8 +459,8 @@ namespace gtsam { // dfR/dVj Matrix3::Zero(); } - if(H5) { + if(H5) { const Matrix3 Jrinv_theta_bc = rightJacobianExpMapSO3inverse(theta_biascorrected); const Matrix3 Jr_JbiasOmegaIncr = rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega * biasOmegaIncr); const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega; From 5a8dab068e38459ca6f204af61f9905c8f6c3590 Mon Sep 17 00:00:00 2001 From: djensen Date: Tue, 4 Feb 2014 11:24:17 -0500 Subject: [PATCH 03/90] Added a matlab test for earth rotation --- .../+imuSimulator/testCoriolis.m | 99 +++++++++++++++++++ 1 file changed, 99 insertions(+) create mode 100644 matlab/unstable_examples/+imuSimulator/testCoriolis.m diff --git a/matlab/unstable_examples/+imuSimulator/testCoriolis.m b/matlab/unstable_examples/+imuSimulator/testCoriolis.m new file mode 100644 index 000000000..d79589ca5 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/testCoriolis.m @@ -0,0 +1,99 @@ +clc +clear all +close all + +import gtsam.*; + +deltaT = 0.1; +timeElapsed = 10; +times = 0:deltaT:timeElapsed; + +%omega = [0;0;7.292115e-5]; % Earth Rotation +omega = [0;0;5*pi/10]; +velocity = [0;0;0]; % initially not moving +accelFixed = [0;0.1;0.1]; % accelerate in the positive z-direction +initialPosition = [0; 1.05; 0]; % start along the positive x-axis + +% Initial state +currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); +currentVelocityFixedGT = velocity; + +currentPoseRotatingGT = currentPoseFixedGT; +currentPoseRotatingFrame = Pose3; + +% Positions +positionsFixedGT = zeros(3, length(times)+1); +positionsRotatingGT = zeros(3, length(times)+1); + +positionsFixedGT(:,1) = currentPoseFixedGT.translation.vector; +positionsRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; + +changePoseRotatingFrame = Pose3.Expmap([omega*deltaT; 0; 0; 0]); + +poses(1).p = currentPoseRotatingFrame.translation.vector; +poses(1).R = currentPoseRotatingFrame.rotation.matrix; + +h = figure(1); + +i = 2; +for t = times + %% Create ground truth trajectory + % Update the pose and velocity + currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ... + + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT); + currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT; + + currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); + + % Rotate pose in fixed frame to get pose in rotating frame + currentPoseRotatingFrame = currentPoseRotatingFrame.compose(changePoseRotatingFrame); + currentPoseRotatingGT = currentPoseFixedGT.transform_to(currentPoseRotatingFrame); + + % Store GT (ground truth) poses + positionsFixedGT(:,i) = currentPoseFixedGT.translation.vector; + positionsRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; + poses(i).p = currentPoseRotatingFrame.translation.vector; + poses(i).R = currentPoseRotatingFrame.rotation.matrix; + + % incremental graphing + figure(h) + plot_trajectory(poses(i),1, '-k', 'Rotating Frame',0.1,0.75,1) + hold on; + plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); + plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r'); + plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'o'); + plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'x'); + plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'or'); + plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'xr'); + hold off; + xlabel('X axis') + ylabel('Y axis') + zlabel('Z axis') + axis equal + grid on; + pause(0.1); + + i = i + 1; +end + + +figure +sphere +hold on; +plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); +plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r'); + +% beginning and end points of Fixed +plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'o'); +plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'x'); + +% beginning and end points of Rotating +plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'or'); +plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'xr'); + +xlabel('X axis') +ylabel('Y axis') +zlabel('Z axis') +axis equal +grid on; +hold off; From f327a4ab46d2942b2b8f584f936521e77425fa27 Mon Sep 17 00:00:00 2001 From: djensen Date: Wed, 5 Feb 2014 09:43:05 -0500 Subject: [PATCH 04/90] Renamed 'testCoriolis' to 'coriolisExample' and added comments --- .../{testCoriolis.m => coriolisExample.m} | 31 ++++++++++++++----- 1 file changed, 23 insertions(+), 8 deletions(-) rename matlab/unstable_examples/+imuSimulator/{testCoriolis.m => coriolisExample.m} (70%) diff --git a/matlab/unstable_examples/+imuSimulator/testCoriolis.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m similarity index 70% rename from matlab/unstable_examples/+imuSimulator/testCoriolis.m rename to matlab/unstable_examples/+imuSimulator/coriolisExample.m index d79589ca5..9599c2508 100644 --- a/matlab/unstable_examples/+imuSimulator/testCoriolis.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -1,27 +1,38 @@ +%% coriolisExample.m +% Author(s): David Jensen (david.jensen@gtri.gatech.edu) +% This script demonstrates the relationship between object motion in inertial and rotating reference frames. +% For this example, we consider a fixed (inertial) reference frame located at the center of the earth and initially +% coincident with the rotating ECEF reference frame (X towards 0 longitude, Y towards 90 longitude, Z towards 90 latitude), +% which rotates with the earth. +% A body is moving in the positive Z-direction and positive Y-direction with respect to the fixed reference frame. +% Its position is plotted in both the fixed and rotating reference frames to simulate how an observer in each frame would +% experience the body's motion. + clc clear all close all import gtsam.*; +%% General configuration deltaT = 0.1; timeElapsed = 10; times = 0:deltaT:timeElapsed; %omega = [0;0;7.292115e-5]; % Earth Rotation omega = [0;0;5*pi/10]; -velocity = [0;0;0]; % initially not moving -accelFixed = [0;0.1;0.1]; % accelerate in the positive z-direction +velocity = [0;0;0]; % initially not moving +accelFixed = [0;0.1;0.1]; % accelerate in the positive z-direction initialPosition = [0; 1.05; 0]; % start along the positive x-axis -% Initial state +%% Initial state of the body in the fixed in rotating frames should be the same currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); currentVelocityFixedGT = velocity; currentPoseRotatingGT = currentPoseFixedGT; currentPoseRotatingFrame = Pose3; -% Positions +%% Initialize storage variables positionsFixedGT = zeros(3, length(times)+1); positionsRotatingGT = zeros(3, length(times)+1); @@ -35,10 +46,13 @@ poses(1).R = currentPoseRotatingFrame.rotation.matrix; h = figure(1); +%% Main loop: iterate through i = 2; for t = times %% Create ground truth trajectory - % Update the pose and velocity + % Update the position and velocity + % x_t = x_0 + v_0*dt + 1/2*a_0*dt^2 + % v_t = v_0 + a_0*dt currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ... + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT); currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT; @@ -55,7 +69,7 @@ for t = times poses(i).p = currentPoseRotatingFrame.translation.vector; poses(i).R = currentPoseRotatingFrame.rotation.matrix; - % incremental graphing + % incremental plotting for animation figure(h) plot_trajectory(poses(i),1, '-k', 'Rotating Frame',0.1,0.75,1) hold on; @@ -76,10 +90,11 @@ for t = times i = i + 1; end - +%% Final plotting figure -sphere +sphere % sphere for reference hold on; +% trajectories in Fixed and Rotating frames plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r'); From 3e7e4d19f61d20ff33488385a4912855a6083c8a Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 6 Feb 2014 15:10:36 -0500 Subject: [PATCH 05/90] coriolisExample: added basic IMU inference and fixed a plotting bug --- .../+imuSimulator/coriolisExample.m | 168 +++++++++++++----- 1 file changed, 128 insertions(+), 40 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 9599c2508..63464da2c 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -1,7 +1,7 @@ %% coriolisExample.m % Author(s): David Jensen (david.jensen@gtri.gatech.edu) % This script demonstrates the relationship between object motion in inertial and rotating reference frames. -% For this example, we consider a fixed (inertial) reference frame located at the center of the earth and initially +% For this example, we consider a fixed (inertial) reference frame located at the center of the earth and initially % coincident with the rotating ECEF reference frame (X towards 0 longitude, Y towards 90 longitude, Z towards 90 latitude), % which rotates with the earth. % A body is moving in the positive Z-direction and positive Y-direction with respect to the fixed reference frame. @@ -14,16 +14,29 @@ close all import gtsam.*; +addpath(genpath('./Libraries')) + %% General configuration deltaT = 0.1; timeElapsed = 10; times = 0:deltaT:timeElapsed; %omega = [0;0;7.292115e-5]; % Earth Rotation -omega = [0;0;5*pi/10]; +%omega = [0;0;5*pi/10]; +omega = [0;0;0]; +omegaFixed = [0;0;0]; velocity = [0;0;0]; % initially not moving accelFixed = [0;0.1;0.1]; % accelerate in the positive z-direction initialPosition = [0; 1.05; 0]; % start along the positive x-axis +IMUinBody = Pose3; +g = [0;0;0]; % Gravity, will need to fix this b/c of ECEF frame +zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); +IMU_metadata.AccelerometerSigma = 1e-5; +IMU_metadata.GyroscopeSigma = 1e-7; +IMU_metadata.IntegrationSigma = 1e-10; +sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); +sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10); +sigma_init_b = noiseModel.Isotropic.Sigma(6, 1e-10); %% Initial state of the body in the fixed in rotating frames should be the same currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); @@ -33,52 +46,127 @@ currentPoseRotatingGT = currentPoseFixedGT; currentPoseRotatingFrame = Pose3; %% Initialize storage variables -positionsFixedGT = zeros(3, length(times)+1); -positionsRotatingGT = zeros(3, length(times)+1); - -positionsFixedGT(:,1) = currentPoseFixedGT.translation.vector; -positionsRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; +positionsFixedGT = zeros(3, length(times)); +positionsRotatingGT = zeros(3, length(times)); changePoseRotatingFrame = Pose3.Expmap([omega*deltaT; 0; 0; 0]); - -poses(1).p = currentPoseRotatingFrame.translation.vector; -poses(1).R = currentPoseRotatingFrame.rotation.matrix; - h = figure(1); -%% Main loop: iterate through -i = 2; -for t = times - %% Create ground truth trajectory - % Update the position and velocity - % x_t = x_0 + v_0*dt + 1/2*a_0*dt^2 - % v_t = v_0 + a_0*dt - currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ... - + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT); - currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT; +positionsEstimates = zeros(3,length(times)+1); + +% Solver object +isamParams = ISAM2Params; +isamParams.setFactorization('CHOLESKY'); +isamParams.setRelinearizeSkip(10); +isam = gtsam.ISAM2(isamParams); +newFactors = NonlinearFactorGraph; +newValues = Values; + + +%% Main loop: iterate through +for i = 1:length(times) + t = times(i); - currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); + % Create keys for current state + currentPoseKey = symbol('x', i); + currentVelKey = symbol('v', i); + currentBiasKey = symbol('b', i); - % Rotate pose in fixed frame to get pose in rotating frame - currentPoseRotatingFrame = currentPoseRotatingFrame.compose(changePoseRotatingFrame); - currentPoseRotatingGT = currentPoseFixedGT.transform_to(currentPoseRotatingFrame); - - % Store GT (ground truth) poses - positionsFixedGT(:,i) = currentPoseFixedGT.translation.vector; - positionsRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; - poses(i).p = currentPoseRotatingFrame.translation.vector; - poses(i).R = currentPoseRotatingFrame.rotation.matrix; - - % incremental plotting for animation + if(i == 1) + positionsFixedGT(:,1) = currentPoseFixedGT.translation.vector; + positionsRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; + poses(1).p = currentPoseRotatingFrame.translation.vector; + poses(1).R = currentPoseRotatingFrame.rotation.matrix; + + currentPoseGlobal = currentPoseFixedGT; + currentVelocityGlobal = LieVector(currentVelocityFixedGT); + + % Set Priors + newValues.insert(currentPoseKey, currentPoseGlobal); + newValues.insert(currentVelKey, currentVelocityGlobal); + newValues.insert(currentBiasKey, zeroBias); + newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); + newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); + + else + + %% Create ground truth trajectory + % Update the position and velocity + % x_t = x_0 + v_0*dt + 1/2*a_0*dt^2 + % v_t = v_0 + a_0*dt + currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ... + + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT); + currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT; + + currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); + + % Rotate pose in fixed frame to get pose in rotating frame + currentPoseRotatingFrame = currentPoseRotatingFrame.compose(changePoseRotatingFrame); + currentPoseRotatingGT = currentPoseFixedGT.transform_to(currentPoseRotatingFrame); + + % Store GT (ground truth) poses + positionsFixedGT(:,i) = currentPoseFixedGT.translation.vector; + positionsRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; + poses(i).p = currentPoseRotatingFrame.translation.vector; + poses(i).R = currentPoseRotatingFrame.rotation.matrix; + + %% Estimate trajectory in rotating frame using the ground truth + % measurements + % Instantiate preintegrated measurements class + currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... + zeroBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... + IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3)); + % Add measurement + currentSummarizedMeasurement.integrateMeasurement(accelFixed, omegaFixed, deltaT); + % Add factors to graph + newFactors.add(ImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentSummarizedMeasurement, g, omega)); + + %newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); + newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... + noiseModel.Isotropic.Sigma(6, 1e-10))); + + % Add values to the graph + newValues.insert(currentPoseKey, currentPoseGlobal); + newValues.insert(currentVelKey, currentVelocityGlobal); + newValues.insert(currentBiasKey, zeroBias); + + %newFactors.print(''); + %newValues.print(''); + + %% Solve factor graph + if(i > 1) + isam.update(newFactors, newValues); + newFactors = NonlinearFactorGraph; + newValues = Values; + + currentPoseGlobal = isam.calculateEstimate(currentPoseKey); + currentVelocityGlobal = isam.calculateEstimate(currentVelKey); + currentBias = isam.calculateEstimate(currentBiasKey); + + positionsEstimates(:,i) = currentPoseGlobal.translation.vector; + %velocitiesEstimates(:,i) = currentVelocityGlobal; + end + end + %% incremental plotting for animation (ground truth) figure(h) plot_trajectory(poses(i),1, '-k', 'Rotating Frame',0.1,0.75,1) hold on; - plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); - plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r'); + plot3(positionsFixedGT(1,1:i), positionsFixedGT(2,1:i), positionsFixedGT(3,1:i)); plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'o'); plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'x'); + + plot3(positionsRotatingGT(1,1:i), positionsRotatingGT(2,1:i), positionsRotatingGT(3,1:i), '-r'); plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'or'); plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'xr'); + + plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), '-g'); + plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'og'); + plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'xg'); + hold off; xlabel('X axis') ylabel('Y axis') @@ -99,16 +187,16 @@ plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r'); % beginning and end points of Fixed -plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'o'); -plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'x'); +plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'x'); +plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'o'); % beginning and end points of Rotating -plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'or'); -plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'xr'); +plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'xr'); +plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'or'); xlabel('X axis') ylabel('Y axis') zlabel('Z axis') axis equal grid on; -hold off; +hold off; \ No newline at end of file From 1b13c14d79cf43439c00c11401e4ce2c2e7a48e9 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 6 Feb 2014 17:02:51 -0500 Subject: [PATCH 06/90] Added error plots to coriolisExample --- .../+imuSimulator/coriolisExample.m | 87 ++++++++++++------- 1 file changed, 54 insertions(+), 33 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 63464da2c..c6c532745 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -22,14 +22,14 @@ timeElapsed = 10; times = 0:deltaT:timeElapsed; %omega = [0;0;7.292115e-5]; % Earth Rotation -%omega = [0;0;5*pi/10]; +omega = [0;0;pi/30]; omega = [0;0;0]; omegaFixed = [0;0;0]; velocity = [0;0;0]; % initially not moving -accelFixed = [0;0.1;0.1]; % accelerate in the positive z-direction +accelFixed = [0.1;0.1;0.1]; % accelerate in the positive z-direction initialPosition = [0; 1.05; 0]; % start along the positive x-axis IMUinBody = Pose3; -g = [0;0;0]; % Gravity, will need to fix this b/c of ECEF frame +g = [0;0;0]; % Gravity zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.GyroscopeSigma = 1e-7; @@ -48,12 +48,11 @@ currentPoseRotatingFrame = Pose3; %% Initialize storage variables positionsFixedGT = zeros(3, length(times)); positionsRotatingGT = zeros(3, length(times)); +positionsEstimates = zeros(3,length(times)); changePoseRotatingFrame = Pose3.Expmap([omega*deltaT; 0; 0; 0]); h = figure(1); -positionsEstimates = zeros(3,length(times)+1); - % Solver object isamParams = ISAM2Params; isamParams.setFactorization('CHOLESKY'); @@ -63,7 +62,8 @@ newFactors = NonlinearFactorGraph; newValues = Values; -%% Main loop: iterate through +%% Main loop: iterate through the ground truth trajectory, add factors +% and values to the factor graph, and perform inference for i = 1:length(times) t = times(i); @@ -72,23 +72,27 @@ for i = 1:length(times) currentVelKey = symbol('v', i); currentBiasKey = symbol('b', i); + %% Set priors on the first iteration if(i == 1) positionsFixedGT(:,1) = currentPoseFixedGT.translation.vector; positionsRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; poses(1).p = currentPoseRotatingFrame.translation.vector; poses(1).R = currentPoseRotatingFrame.rotation.matrix; - currentPoseGlobal = currentPoseFixedGT; - currentVelocityGlobal = LieVector(currentVelocityFixedGT); + currentPoseEstimate = currentPoseFixedGT; + currentVelocityEstimate = LieVector(currentVelocityFixedGT); % Set Priors - newValues.insert(currentPoseKey, currentPoseGlobal); - newValues.insert(currentVelKey, currentVelocityGlobal); + newValues.insert(currentPoseKey, currentPoseEstimate); + newValues.insert(currentVelKey, currentVelocityEstimate); newValues.insert(currentBiasKey, zeroBias); - newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); - newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x)); + newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); + % Store data + positionsEstimates(:,i) = currentPoseEstimate.translation.vector; + else %% Create ground truth trajectory @@ -111,8 +115,7 @@ for i = 1:length(times) poses(i).p = currentPoseRotatingFrame.translation.vector; poses(i).R = currentPoseRotatingFrame.rotation.matrix; - %% Estimate trajectory in rotating frame using the ground truth - % measurements + %% Estimate trajectory in rotating frame using the ground truth measurements % Instantiate preintegrated measurements class currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... zeroBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... @@ -129,9 +132,11 @@ for i = 1:length(times) newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... noiseModel.Isotropic.Sigma(6, 1e-10))); - % Add values to the graph - newValues.insert(currentPoseKey, currentPoseGlobal); - newValues.insert(currentVelKey, currentVelocityGlobal); + % Add values to the graph. Use the current pose and velocity + % estimates as to values when interpreting the next pose and + % velocity estimates + newValues.insert(currentPoseKey, currentPoseEstimate); + newValues.insert(currentVelKey, currentVelocityEstimate); newValues.insert(currentBiasKey, zeroBias); %newFactors.print(''); @@ -143,29 +148,31 @@ for i = 1:length(times) newFactors = NonlinearFactorGraph; newValues = Values; - currentPoseGlobal = isam.calculateEstimate(currentPoseKey); - currentVelocityGlobal = isam.calculateEstimate(currentVelKey); + % Get the new pose, velocity, and bias estimates + currentPoseEstimate = isam.calculateEstimate(currentPoseKey); + currentVelocityEstimate = isam.calculateEstimate(currentVelKey); currentBias = isam.calculateEstimate(currentBiasKey); - positionsEstimates(:,i) = currentPoseGlobal.translation.vector; + positionsEstimates(:,i) = currentPoseEstimate.translation.vector; %velocitiesEstimates(:,i) = currentVelocityGlobal; end end + %% incremental plotting for animation (ground truth) figure(h) plot_trajectory(poses(i),1, '-k', 'Rotating Frame',0.1,0.75,1) hold on; plot3(positionsFixedGT(1,1:i), positionsFixedGT(2,1:i), positionsFixedGT(3,1:i)); - plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'o'); - plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'x'); + plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'x'); + plot3(positionsFixedGT(1,i), positionsFixedGT(2,i), positionsFixedGT(3,i), 'o'); plot3(positionsRotatingGT(1,1:i), positionsRotatingGT(2,1:i), positionsRotatingGT(3,1:i), '-r'); - plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'or'); - plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'xr'); + plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'xr'); + plot3(positionsRotatingGT(1,i), positionsRotatingGT(2,i), positionsRotatingGT(3,i), 'or'); plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), '-g'); - plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'og'); - plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'xg'); + plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xg'); + plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'og'); hold off; xlabel('X axis') @@ -173,27 +180,41 @@ for i = 1:length(times) zlabel('Z axis') axis equal grid on; - pause(0.1); + %pause(0.1); i = i + 1; end -%% Final plotting +figure +%% Print and plot trajectory error results +positionsError = positionsRotatingGT - positionsEstimates; +fprintf('Final position error = %f\n', positionsError(end)); +plot(times, positionsError); +plotTitle = sprintf('Error in Estimated Position (omega = [%.2f; %.2f; %.2f])', omega(1), omega(2), omega(3)); +title(plotTitle); +xlabel('Time'); +ylabel('Error (ground_truth - estimate)'); +legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); + +%% Plot final trajectories figure sphere % sphere for reference hold on; -% trajectories in Fixed and Rotating frames +% Ground truth trajectory in fixed reference frame plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); -plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r'); - -% beginning and end points of Fixed plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'x'); plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'o'); -% beginning and end points of Rotating +% Ground truth trajectory in rotating reference frame +plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r'); plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'xr'); plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'or'); +% Estimates +plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-g'); +plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xg'); +plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'og'); + xlabel('X axis') ylabel('Y axis') zlabel('Z axis') From 38e0e411fbf47f9fc2a0763496371d5202ecb431 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Tue, 11 Feb 2014 11:13:13 -0500 Subject: [PATCH 07/90] Added IMU type 2 to coriolis example. --- .../+imuSimulator/coriolisExample.m | 240 ++++++++++++------ 1 file changed, 160 insertions(+), 80 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index c6c532745..fb41114b3 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -17,40 +17,55 @@ import gtsam.*; addpath(genpath('./Libraries')) %% General configuration -deltaT = 0.1; -timeElapsed = 10; +navFrameRotating = 0; +deltaT = 0.01; +timeElapsed = 5; times = 0:deltaT:timeElapsed; +IMU_type = 1; +record_movie = 0; -%omega = [0;0;7.292115e-5]; % Earth Rotation -omega = [0;0;pi/30]; -omega = [0;0;0]; -omegaFixed = [0;0;0]; -velocity = [0;0;0]; % initially not moving -accelFixed = [0.1;0.1;0.1]; % accelerate in the positive z-direction -initialPosition = [0; 1.05; 0]; % start along the positive x-axis -IMUinBody = Pose3; +omegaEarthSeconds = 100*[0;0;7.292115e-5]; % Earth Rotation +% omegaRotatingFrame = omegaEarthSeconds/deltaT;%[0;0;pi/3000]; % rotation of the moving frame wrt fixed frame +omegaRotatingFrame = [0;0;pi/300]; +currentRotatingFrame = Pose3; % initially coincide with fixed frame +omegaFixed = [0;0;0]; % constant rotation rate measurement +accelFixed =10 * [0;0;0.1]; % constant acceleration measurement g = [0;0;0]; % Gravity -zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); + +if navFrameRotating == 0 + omegaCoriolisIMU = [0;0;0]; +else + omegaCoriolisIMU = omegaRotatingFrame; +end + +% Initial conditions +velocity = [0;0;0]; % initially not moving +initialPosition = [1; 0; 0]; % start along the positive x-axis +% +currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); +currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0 +currentVelocityFixedGT = velocity; +% +epsBias = 1e-15; +sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); +sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10); +sigma_init_b = noiseModel.Isotropic.Sigma(6, epsBias); + +% Imu metadata +zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of interest and is set to zero IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.GyroscopeSigma = 1e-7; IMU_metadata.IntegrationSigma = 1e-10; -sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); -sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10); -sigma_init_b = noiseModel.Isotropic.Sigma(6, 1e-10); - -%% Initial state of the body in the fixed in rotating frames should be the same -currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); -currentVelocityFixedGT = velocity; - -currentPoseRotatingGT = currentPoseFixedGT; -currentPoseRotatingFrame = Pose3; +IMU_metadata.BiasAccelerometerSigma = 1e-5; +IMU_metadata.BiasGyroscopeSigma = 1e-7; +IMU_metadata.BiasAccOmegaInit = 1e-10; %% Initialize storage variables -positionsFixedGT = zeros(3, length(times)); -positionsRotatingGT = zeros(3, length(times)); +positionsInFixedGT = zeros(3, length(times)); +positionsInRotatingGT = zeros(3, length(times)); positionsEstimates = zeros(3,length(times)); -changePoseRotatingFrame = Pose3.Expmap([omega*deltaT; 0; 0; 0]); +changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step h = figure(1); % Solver object @@ -61,6 +76,15 @@ isam = gtsam.ISAM2(isamParams); newFactors = NonlinearFactorGraph; newValues = Values; +% Video recording object +if record_movie == 1 + writerObj = VideoWriter('trajectories.avi'); + writerObj.Quality = 100; + writerObj.FrameRate = 15; %10; + open(writerObj); + set(gca,'nextplot','replacechildren'); + set(gcf,'Renderer','zbuffer'); +end %% Main loop: iterate through the ground truth trajectory, add factors % and values to the factor graph, and perform inference @@ -74,25 +98,24 @@ for i = 1:length(times) %% Set priors on the first iteration if(i == 1) - positionsFixedGT(:,1) = currentPoseFixedGT.translation.vector; - positionsRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; - poses(1).p = currentPoseRotatingFrame.translation.vector; - poses(1).R = currentPoseRotatingFrame.rotation.matrix; + currentPoseEstimate = currentPoseFixedGT; % known initial conditions + currentVelocityEstimate = LieVector(currentVelocityFixedGT); % known initial conditions - currentPoseEstimate = currentPoseFixedGT; - currentVelocityEstimate = LieVector(currentVelocityFixedGT); - % Set Priors newValues.insert(currentPoseKey, currentPoseEstimate); newValues.insert(currentVelKey, currentVelocityEstimate); newValues.insert(currentBiasKey, zeroBias); + % Initial values, same for IMU types 1 and 2 newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x)); newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); % Store data + positionsInFixedGT(:,1) = currentPoseFixedGT.translation.vector; + positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; positionsEstimates(:,i) = currentPoseEstimate.translation.vector; - + currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector; + currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix; else %% Create ground truth trajectory @@ -103,35 +126,70 @@ for i = 1:length(times) + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT); currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT; - currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); + currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation % Rotate pose in fixed frame to get pose in rotating frame - currentPoseRotatingFrame = currentPoseRotatingFrame.compose(changePoseRotatingFrame); - currentPoseRotatingGT = currentPoseFixedGT.transform_to(currentPoseRotatingFrame); + currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame); + %currentPoseRotatingGT = currentPoseFixedGT.transform_to(currentRotatingFrame); + inverseCurrentRotatingFrame = (currentRotatingFrame.inverse); + currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT); + + %inverseCurrentPoseRotatingGT = currentRotatingFrame.rotation.inverse; + %TODO: currentPoseRotatingGT.rotation = inverseCurrentPoseRotatingGT.compose(currentPoseFixedGT.rotation); % Store GT (ground truth) poses - positionsFixedGT(:,i) = currentPoseFixedGT.translation.vector; - positionsRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; - poses(i).p = currentPoseRotatingFrame.translation.vector; - poses(i).R = currentPoseRotatingFrame.rotation.matrix; + positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector; + positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; + currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector; + currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix; - %% Estimate trajectory in rotating frame using the ground truth measurements + %% Estimate trajectory in rotating frame using GTSAM (ground truth measurements) % Instantiate preintegrated measurements class - currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... - zeroBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ... - IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3)); + if IMU_type == 1 + currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... + zeroBias, ... + IMU_metadata.AccelerometerSigma.^2 * eye(3), ... + IMU_metadata.GyroscopeSigma.^2 * eye(3), ... + IMU_metadata.IntegrationSigma.^2 * eye(3)); + elseif IMU_type == 2 + currentSummarizedMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements( ... + zeroBias, ... + IMU_metadata.AccelerometerSigma.^2 * eye(3), ... + IMU_metadata.GyroscopeSigma.^2 * eye(3), ... + IMU_metadata.IntegrationSigma.^2 * eye(3), ... + IMU_metadata.BiasAccelerometerSigma.^2 * eye(3), ... + IMU_metadata.BiasGyroscopeSigma.^2 * eye(3), ... + IMU_metadata.BiasAccOmegaInit.^2 * eye(6)); + else + error('imuSimulator:coriolisExample:IMU_typeNotFound', ... + 'IMU_type = %d does not exist.\nAvailable IMU types are 1 and 2\n', IMU_type); + end + % Add measurement currentSummarizedMeasurement.integrateMeasurement(accelFixed, omegaFixed, deltaT); + % Add factors to graph - newFactors.add(ImuFactor( ... - currentPoseKey-1, currentVelKey-1, ... - currentPoseKey, currentVelKey, ... - currentBiasKey-1, currentSummarizedMeasurement, g, omega)); - - %newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); - newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ... - noiseModel.Isotropic.Sigma(6, 1e-10))); - + if IMU_type == 1 + newFactors.add(ImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentSummarizedMeasurement, g, omegaCoriolisIMU)); + newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ... + noiseModel.Isotropic.Sigma(6, epsBias))); + newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ... + noiseModel.Isotropic.Sigma(6, epsBias))); + elseif IMU_type == 2 + newFactors.add(CombinedImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentBiasKey, ... + currentSummarizedMeasurement, g, omegaCoriolisIMU, ... + noiseModel.Isotropic.Sigma(15, epsBias))); + else + error('imuSimulator:coriolisExample:IMU_typeNotFound', ... + 'IMU_type = %d does not exist.\nAvailable IMU types are 1 and 2\n', IMU_type); + end + % Add values to the graph. Use the current pose and velocity % estimates as to values when interpreting the next pose and % velocity estimates @@ -139,8 +197,7 @@ for i = 1:length(times) newValues.insert(currentVelKey, currentVelocityEstimate); newValues.insert(currentBiasKey, zeroBias); - %newFactors.print(''); - %newValues.print(''); + %newFactors.print(''); newValues.print(''); %% Solve factor graph if(i > 1) @@ -154,25 +211,27 @@ for i = 1:length(times) currentBias = isam.calculateEstimate(currentBiasKey); positionsEstimates(:,i) = currentPoseEstimate.translation.vector; - %velocitiesEstimates(:,i) = currentVelocityGlobal; + velocitiesEstimates(:,i) = currentVelocityEstimate.vector; + biasEstimates(:,i) = currentBias.vector; end end %% incremental plotting for animation (ground truth) figure(h) - plot_trajectory(poses(i),1, '-k', 'Rotating Frame',0.1,0.75,1) + %plot_trajectory(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1) + %hold on; + plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r'); hold on; - plot3(positionsFixedGT(1,1:i), positionsFixedGT(2,1:i), positionsFixedGT(3,1:i)); - plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'x'); - plot3(positionsFixedGT(1,i), positionsFixedGT(2,i), positionsFixedGT(3,i), 'o'); + %plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'x'); + %plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'o'); - plot3(positionsRotatingGT(1,1:i), positionsRotatingGT(2,1:i), positionsRotatingGT(3,1:i), '-r'); - plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'xr'); - plot3(positionsRotatingGT(1,i), positionsRotatingGT(2,i), positionsRotatingGT(3,i), 'or'); + plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), '-g'); + %plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); + %plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og'); - plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), '-g'); - plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xg'); - plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'og'); + plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), '-b'); + %plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); + %plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); hold off; xlabel('X axis') @@ -182,42 +241,63 @@ for i = 1:length(times) grid on; %pause(0.1); - i = i + 1; + if record_movie == 1 + frame = getframe(gcf); + writeVideo(writerObj,frame); + end +end + +if record_movie == 1 + close(writerObj); end figure %% Print and plot trajectory error results -positionsError = positionsRotatingGT - positionsEstimates; -fprintf('Final position error = %f\n', positionsError(end)); -plot(times, positionsError); -plotTitle = sprintf('Error in Estimated Position (omega = [%.2f; %.2f; %.2f])', omega(1), omega(2), omega(3)); +if navFrameRotating == 0 + axisPositionsError = positionsInFixedGT - positionsEstimates; +else + axisPositionsError = positionsInRotatingGT - positionsEstimates; +end +plot(times, abs(axisPositionsError)); +plotTitle = sprintf('Axis Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); title(plotTitle); xlabel('Time'); ylabel('Error (ground_truth - estimate)'); legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); +figure +positionError3D = sqrt(axisPositionsError(1,:).^2+axisPositionsError(2,:).^2 + axisPositionsError(3,:).^2); +plot(times, positionError3D); +plotTitle = sprintf('3D Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time'); +ylabel('3D error [meters]'); +fprintf('Final position error = %f\n', norm(axisPositionsError(:,end))); + %% Plot final trajectories figure sphere % sphere for reference hold on; % Ground truth trajectory in fixed reference frame -plot3(positionsFixedGT(1,:), positionsFixedGT(2,:), positionsFixedGT(3,:)); -plot3(positionsFixedGT(1,1), positionsFixedGT(2,1), positionsFixedGT(3,1), 'x'); -plot3(positionsFixedGT(1,end), positionsFixedGT(2,end), positionsFixedGT(3,end), 'o'); +plot3(positionsInFixedGT(1,:), positionsInFixedGT(2,:), positionsInFixedGT(3,:),'r'); +plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr'); +plot3(positionsInFixedGT(1,end), positionsInFixedGT(2,end), positionsInFixedGT(3,end), 'or'); % Ground truth trajectory in rotating reference frame -plot3(positionsRotatingGT(1,:), positionsRotatingGT(2,:), positionsRotatingGT(3,:), '-r'); -plot3(positionsRotatingGT(1,1), positionsRotatingGT(2,1), positionsRotatingGT(3,1), 'xr'); -plot3(positionsRotatingGT(1,end), positionsRotatingGT(2,end), positionsRotatingGT(3,end), 'or'); +plot3(positionsInRotatingGT(1,:), positionsInRotatingGT(2,:), positionsInRotatingGT(3,:), '-g'); +plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); +plot3(positionsInRotatingGT(1,end), positionsInRotatingGT(2,end), positionsInRotatingGT(3,end), 'og'); % Estimates -plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-g'); -plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xg'); -plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'og'); +plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-b'); +plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); +plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'ob'); xlabel('X axis') ylabel('Y axis') zlabel('Z axis') axis equal grid on; -hold off; \ No newline at end of file +hold off; From e43ece27eebdde6fe3b6439b672e8c5b0aa24fc7 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 12 Feb 2014 13:16:44 -0500 Subject: [PATCH 08/90] Additional output messages and trajectory length calculations --- .../+imuSimulator/coriolisExample.m | 60 ++++++++++++++++--- 1 file changed, 53 insertions(+), 7 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index fb41114b3..f17e350a5 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -17,19 +17,25 @@ import gtsam.*; addpath(genpath('./Libraries')) %% General configuration -navFrameRotating = 0; -deltaT = 0.01; -timeElapsed = 5; +navFrameRotating = 1; % 0 = perform navigation in the fixed frame + % 1 = perform navigation in the rotating frame +IMU_type = 1; % IMU type 1 or type 2 +useRealisticValues = 0; % use reaslist values for initial position and earth rotation +record_movie = 0; % 0 = do not record movie + % 1 = record movie of the trajectories. One + % frame per time step (15 fps) + +%% Scenario Configuration +deltaT = 0.01; % timestep +timeElapsed = 5; % Total elapsed time times = 0:deltaT:timeElapsed; -IMU_type = 1; -record_movie = 0; omegaEarthSeconds = 100*[0;0;7.292115e-5]; % Earth Rotation % omegaRotatingFrame = omegaEarthSeconds/deltaT;%[0;0;pi/3000]; % rotation of the moving frame wrt fixed frame omegaRotatingFrame = [0;0;pi/300]; currentRotatingFrame = Pose3; % initially coincide with fixed frame omegaFixed = [0;0;0]; % constant rotation rate measurement -accelFixed =10 * [0;0;0.1]; % constant acceleration measurement +accelFixed =10 * [0.1;0.1;0.1]; % constant acceleration measurement g = [0;0;0]; % Gravity if navFrameRotating == 0 @@ -86,6 +92,24 @@ if record_movie == 1 set(gcf,'Renderer','zbuffer'); end +%% Print Info about the test +fprintf('\n-------------------------------------------------'); +if navFrameRotating == 0 + fprintf('Performing navigation in the Fixed frame.\n'); +else + fprintf('\nPerforming navigation in the Rotating frame.\n'); +end +fprintf('IMU_type = %d\n', IMU_type); +fprintf('Record Movie = %d\n', record_movie); +fprintf('Realistic Values = %d\n', useRealisticValues); +fprintf('deltaT = %f\n', deltaT); +fprintf('timeElapsed = %f\n', timeElapsed); +fprintf('omegaRotatingFrame = [%f %f %f]\n', omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +fprintf('omegaFixed = [%f %f %f]\n', omegaFixed(1), omegaFixed(2), omegaFixed(3)); +fprintf('accelFixed = [%f %f %f]\n', accelFixed(1), accelFixed(2), accelFixed(3)); +fprintf('Initial Velocity = [%f %f %f]\n', velocity(1), velocity(2), velocity(3)); +fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3)); +fprintf('\n'); %% Main loop: iterate through the ground truth trajectory, add factors % and values to the factor graph, and perform inference for i = 1:length(times) @@ -251,6 +275,16 @@ if record_movie == 1 close(writerObj); end +% Calculate trajectory length +trajectoryLengthEstimated = 0; +trajectoryLengthFixedFrameGT = 0; +trajectoryLengthRotatingFrameGT = 0; +for i = 2:length(positionsEstimates) + trajectoryLengthEstimated = trajectoryLengthEstimated + norm(positionsEstimates(:,i) - positionsEstimates(:,i-1)); + trajectoryLengthFixedFrameGT = trajectoryLengthFixedFrameGT + norm(positionsInFixedGT(:,i) - positionsInFixedGT(:,i-1)); + trajectoryLengthRotatingFrameGT = trajectoryLengthRotatingFrameGT + norm(positionsInRotatingGT(:,i) - positionsInRotatingGT(:,i-1)); +end + figure %% Print and plot trajectory error results if navFrameRotating == 0 @@ -274,7 +308,19 @@ plotTitle = sprintf('3D Error in Estimated Position\n(IMU type = %d, omega = [%. title(plotTitle); xlabel('Time'); ylabel('3D error [meters]'); -fprintf('Final position error = %f\n', norm(axisPositionsError(:,end))); + +if navFrameRotating == 0 + fprintf('Fixed Frame ground truth trajectory length is %f [m]\n', trajectoryLengthFixedFrameGT); + fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated); + fprintf('Final position error = %f (%.4f%% of ground truth trajectory length)\n', ... + norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100); +else + fprintf('Rotating Frame ground truth trajectory length is %f [m]\n', trajectoryLengthRotatingFrameGT); + fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated); + fprintf('Final position error = %f (%.4f%% of ground truth trajectory length)\n', ... + norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthRotatingFrameGT*100); +end + %% Plot final trajectories figure From 7b9008933ba4517755cbedcb4853d66182fe54cc Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 12 Feb 2014 18:34:33 -0500 Subject: [PATCH 09/90] Added realistic values test to coriolisExample.m --- .../+imuSimulator/coriolisExample.m | 38 +++++++++++-------- 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index f17e350a5..23751510c 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -20,7 +20,7 @@ addpath(genpath('./Libraries')) navFrameRotating = 1; % 0 = perform navigation in the fixed frame % 1 = perform navigation in the rotating frame IMU_type = 1; % IMU type 1 or type 2 -useRealisticValues = 0; % use reaslist values for initial position and earth rotation +useRealisticValues = 1; % use reaslist values for initial position and earth rotation record_movie = 0; % 0 = do not record movie % 1 = record movie of the trajectories. One % frame per time step (15 fps) @@ -30,13 +30,23 @@ deltaT = 0.01; % timestep timeElapsed = 5; % Total elapsed time times = 0:deltaT:timeElapsed; -omegaEarthSeconds = 100*[0;0;7.292115e-5]; % Earth Rotation -% omegaRotatingFrame = omegaEarthSeconds/deltaT;%[0;0;pi/3000]; % rotation of the moving frame wrt fixed frame -omegaRotatingFrame = [0;0;pi/300]; -currentRotatingFrame = Pose3; % initially coincide with fixed frame -omegaFixed = [0;0;0]; % constant rotation rate measurement -accelFixed =10 * [0.1;0.1;0.1]; % constant acceleration measurement -g = [0;0;0]; % Gravity +% Initial Conditions +omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s) +if useRealisticValues == 1 + omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [0.5;-0.5;0]; % constant acceleration measurement + g = [0;0;0]; % Gravity + initialVelocity = [0;0;0]; % initial velocity + initialPosition = [4509997.76107; 4509997.76107; 3189050]; % initial position +else + omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [0.1;0;0]; % constant acceleration measurement + g = [0;0;0]; % Gravity + initialVelocity = [0;0;0]; % initial velocity + initialPosition = [0; 1; 0]; % initial position, at 45 degrees longitude and 30 degrees latitude on earth surface +end if navFrameRotating == 0 omegaCoriolisIMU = [0;0;0]; @@ -44,13 +54,11 @@ else omegaCoriolisIMU = omegaRotatingFrame; end -% Initial conditions -velocity = [0;0;0]; % initially not moving -initialPosition = [1; 0; 0]; % start along the positive x-axis % +currentRotatingFrame = Pose3; % rotating frame initially coincides with fixed frame at t=0 currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0 -currentVelocityFixedGT = velocity; +currentVelocityFixedGT = initialVelocity; % epsBias = 1e-15; sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); @@ -93,11 +101,11 @@ if record_movie == 1 end %% Print Info about the test -fprintf('\n-------------------------------------------------'); +fprintf('\n-------------------------------------------------\n'); if navFrameRotating == 0 fprintf('Performing navigation in the Fixed frame.\n'); else - fprintf('\nPerforming navigation in the Rotating frame.\n'); + fprintf('Performing navigation in the Rotating frame.\n'); end fprintf('IMU_type = %d\n', IMU_type); fprintf('Record Movie = %d\n', record_movie); @@ -107,7 +115,7 @@ fprintf('timeElapsed = %f\n', timeElapsed); fprintf('omegaRotatingFrame = [%f %f %f]\n', omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); fprintf('omegaFixed = [%f %f %f]\n', omegaFixed(1), omegaFixed(2), omegaFixed(3)); fprintf('accelFixed = [%f %f %f]\n', accelFixed(1), accelFixed(2), accelFixed(3)); -fprintf('Initial Velocity = [%f %f %f]\n', velocity(1), velocity(2), velocity(3)); +fprintf('Initial Velocity = [%f %f %f]\n', initialVelocity(1), initialVelocity(2), initialVelocity(3)); fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3)); fprintf('\n'); %% Main loop: iterate through the ground truth trajectory, add factors From d126fc8f24d0d6761e943a39495135ae04fe4828 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 13 Feb 2014 09:42:42 -0500 Subject: [PATCH 10/90] Added velocity error plots to coriolisExample --- .../+imuSimulator/coriolisExample.m | 47 ++++++++++++++----- 1 file changed, 35 insertions(+), 12 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 23751510c..3c82f5d7b 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -20,7 +20,7 @@ addpath(genpath('./Libraries')) navFrameRotating = 1; % 0 = perform navigation in the fixed frame % 1 = perform navigation in the rotating frame IMU_type = 1; % IMU type 1 or type 2 -useRealisticValues = 1; % use reaslist values for initial position and earth rotation +useRealisticValues = 0; % use reaslist values for initial position and earth rotation record_movie = 0; % 0 = do not record movie % 1 = record movie of the trajectories. One % frame per time step (15 fps) @@ -76,8 +76,14 @@ IMU_metadata.BiasAccOmegaInit = 1e-10; %% Initialize storage variables positionsInFixedGT = zeros(3, length(times)); +velocityInFixedGT = zeros(3, length(times)); + positionsInRotatingGT = zeros(3, length(times)); +velocityInRotatingGT = zeros(3, length(times)); + positionsEstimates = zeros(3,length(times)); +velocityEstimates = zeros(3,length(times)); +%rotationsEstimates = zeros(3,length(times)); changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step h = figure(1); @@ -144,7 +150,9 @@ for i = 1:length(times) % Store data positionsInFixedGT(:,1) = currentPoseFixedGT.translation.vector; + velocityInFixedGT(:,1) = currentVelocityFixedGT; positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; + %velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity.vector; positionsEstimates(:,i) = currentPoseEstimate.translation.vector; currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector; currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix; @@ -162,16 +170,19 @@ for i = 1:length(times) % Rotate pose in fixed frame to get pose in rotating frame currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame); - %currentPoseRotatingGT = currentPoseFixedGT.transform_to(currentRotatingFrame); - inverseCurrentRotatingFrame = (currentRotatingFrame.inverse); + inverseCurrentRotatingFrame = (currentRotatingFrame.inverse); currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT); - %inverseCurrentPoseRotatingGT = currentRotatingFrame.rotation.inverse; - %TODO: currentPoseRotatingGT.rotation = inverseCurrentPoseRotatingGT.compose(currentPoseFixedGT.rotation); + % Get velocity in rotating frame by treating it like a position and using compose + % Maybe Luca knows a better way to do this within GTSAM. + currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT)); + currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT); % Store GT (ground truth) poses positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector; + velocityInFixedGT(:,i) = currentVelocityFixedGT; positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; + velocityInRotatingGT(:,i) = currentVelocityPoseRotatingGT.translation.vector; currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector; currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix; @@ -243,6 +254,7 @@ for i = 1:length(times) currentBias = isam.calculateEstimate(currentBiasKey); positionsEstimates(:,i) = currentPoseEstimate.translation.vector; + %rotationsEstimates(:,i) = currentPoseEstimate.rotation.vector; velocitiesEstimates(:,i) = currentVelocityEstimate.vector; biasEstimates(:,i) = currentBias.vector; end @@ -293,19 +305,21 @@ for i = 2:length(positionsEstimates) trajectoryLengthRotatingFrameGT = trajectoryLengthRotatingFrameGT + norm(positionsInRotatingGT(:,i) - positionsInRotatingGT(:,i-1)); end -figure -%% Print and plot trajectory error results +%% Print and plot error results if navFrameRotating == 0 axisPositionsError = positionsInFixedGT - positionsEstimates; + axisVelocityError = velocityInFixedGT - velocityEstimates; else axisPositionsError = positionsInRotatingGT - positionsEstimates; + axisVelocityError = velocityInRotatingGT - velocityEstimates; end -plot(times, abs(axisPositionsError)); +figure +plot(times, axisPositionsError); plotTitle = sprintf('Axis Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); title(plotTitle); xlabel('Time'); -ylabel('Error (ground_truth - estimate)'); +ylabel('Error (ground_truth - estimate) [m]'); legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); figure @@ -317,18 +331,27 @@ title(plotTitle); xlabel('Time'); ylabel('3D error [meters]'); +figure +plot(times, axisVelocityError); +plotTitle = sprintf('Axis Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time'); +ylabel('Error (ground_truth - estimate) [m/s]'); +legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); + if navFrameRotating == 0 fprintf('Fixed Frame ground truth trajectory length is %f [m]\n', trajectoryLengthFixedFrameGT); fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated); - fprintf('Final position error = %f (%.4f%% of ground truth trajectory length)\n', ... + fprintf('Final position error = %f [m](%.4f%% of ground truth trajectory length)\n', ... norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100); else fprintf('Rotating Frame ground truth trajectory length is %f [m]\n', trajectoryLengthRotatingFrameGT); fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated); - fprintf('Final position error = %f (%.4f%% of ground truth trajectory length)\n', ... + fprintf('Final position error = %f [m](%.4f%% of ground truth trajectory length)\n', ... norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthRotatingFrameGT*100); end - +fprintf('Final velocity error = [%f %f %f] [m/s]\n', axisVelocityError(1,end), axisVelocityError(2,end), axisVelocityError(3,end)); %% Plot final trajectories figure From 0fabfc39c27c5de862dc9d25f9ba8c59d3166494 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 13 Feb 2014 10:33:45 -0500 Subject: [PATCH 11/90] TODO list --- .../+imuSimulator/coriolisExample.m | 31 +++++++++++++------ 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 3c82f5d7b..1af1ef937 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -37,8 +37,8 @@ if useRealisticValues == 1 omegaFixed = [0;0;0]; % constant rotation rate measurement accelFixed = [0.5;-0.5;0]; % constant acceleration measurement g = [0;0;0]; % Gravity - initialVelocity = [0;0;0]; % initial velocity - initialPosition = [4509997.76107; 4509997.76107; 3189050]; % initial position + initialVelocity = radiusEarth * omegaEarthSeconds [vector.. is a cross product, wee wiki link] ; %[0;0;0]; % TODO % initial velocity + initialPosition = [4509997.76107; 4509997.76107; 3189050]; % initial position (Earth radius 6371 km) else omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame omegaFixed = [0;0;0]; % constant rotation rate measurement @@ -60,7 +60,7 @@ currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0 currentVelocityFixedGT = initialVelocity; % -epsBias = 1e-15; +epsBias = 1e-20; sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10); sigma_init_b = noiseModel.Isotropic.Sigma(6, epsBias); @@ -70,9 +70,9 @@ zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of intere IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.GyroscopeSigma = 1e-7; IMU_metadata.IntegrationSigma = 1e-10; -IMU_metadata.BiasAccelerometerSigma = 1e-5; -IMU_metadata.BiasGyroscopeSigma = 1e-7; -IMU_metadata.BiasAccOmegaInit = 1e-10; +IMU_metadata.BiasAccelerometerSigma = epsBias; +IMU_metadata.BiasGyroscopeSigma = epsBias; +IMU_metadata.BiasAccOmegaInit = epsBias; %% Initialize storage variables positionsInFixedGT = zeros(3, length(times)); @@ -228,9 +228,7 @@ for i = 1:length(times) currentBiasKey-1, currentBiasKey, ... currentSummarizedMeasurement, g, omegaCoriolisIMU, ... noiseModel.Isotropic.Sigma(15, epsBias))); - else - error('imuSimulator:coriolisExample:IMU_typeNotFound', ... - 'IMU_type = %d does not exist.\nAvailable IMU types are 1 and 2\n', IMU_type); + % TODO: prior on biases? end % Add values to the graph. Use the current pose and velocity @@ -378,3 +376,18 @@ zlabel('Z axis') axis equal grid on; hold off; + +% TODO: logging rotation errors +for all time steps + Rerror = Rgt' * Restimated; + % transforming rotation matrix to axis-angle representation + vector_error = Rot3.Logmap(Rerror); + norm(vector_error) + + axis angle: [u,theta], with norm(u)=1 + vector_error = u * theta; + +% TODO: logging velocity errors +velocities.. + + From ed1bcb276115e0d9d2c3573709c1d3706c2ce65a Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 13 Feb 2014 14:34:38 -0500 Subject: [PATCH 12/90] Fixed initial velocity --- .../+imuSimulator/coriolisExample.m | 81 +++++++++++++------ 1 file changed, 55 insertions(+), 26 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 1af1ef937..f783e29f7 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -20,7 +20,7 @@ addpath(genpath('./Libraries')) navFrameRotating = 1; % 0 = perform navigation in the fixed frame % 1 = perform navigation in the rotating frame IMU_type = 1; % IMU type 1 or type 2 -useRealisticValues = 0; % use reaslist values for initial position and earth rotation +useRealisticValues = 1; % use reaslist values for initial position and earth rotation record_movie = 0; % 0 = do not record movie % 1 = record movie of the trajectories. One % frame per time step (15 fps) @@ -32,22 +32,43 @@ times = 0:deltaT:timeElapsed; % Initial Conditions omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s) +radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km +angularVelocityTensorEarth = [ 0 -omegaEarthSeconds(3) omegaEarthSeconds(2); + omegaEarthSeconds(3) 0 -omegaEarthSeconds(1); + -omegaEarthSeconds(2) omegaEarthSeconds(1) 0 ]; if useRealisticValues == 1 omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame omegaFixed = [0;0;0]; % constant rotation rate measurement - accelFixed = [0.5;-0.5;0]; % constant acceleration measurement + accelFixed = [0.5;-0.5;0]; % constant acceleration measurement g = [0;0;0]; % Gravity - initialVelocity = radiusEarth * omegaEarthSeconds [vector.. is a cross product, wee wiki link] ; %[0;0;0]; % TODO % initial velocity - initialPosition = [4509997.76107; 4509997.76107; 3189050]; % initial position (Earth radius 6371 km) + initialLongitude = 45; + initialLatitude = 30; + % initial position at some [longitude, latitude] location on earth's + % surface (approximating Earth as a sphere) + initialPosition = [radiusEarth*sind(initialLongitude); + radiusEarth*cosd(initialLongitude); + radiusEarth*sind(initialLatitude)]; + initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, + % (ignoring the velocity due to the earth's rotation) else omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame omegaFixed = [0;0;0]; % constant rotation rate measurement accelFixed = [0.1;0;0]; % constant acceleration measurement g = [0;0;0]; % Gravity - initialVelocity = [0;0;0]; % initial velocity - initialPosition = [0; 1; 0]; % initial position, at 45 degrees longitude and 30 degrees latitude on earth surface + initialPosition = [0; 1; 0];% initial position in both frames + initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) end +% From Wikipedia Angular Velocity page, dr/dt = W*r, where r is +% position vector and W is angular velocity tensor +% We add the initial velocity in the rotating frame because they +% are the same frame at t=0, so no transformation is needed +angularVelocityTensor = [ 0 -omegaRotatingFrame(3) omegaRotatingFrame(2); + omegaRotatingFrame(3) 0 -omegaRotatingFrame(1); + -omegaRotatingFrame(2) omegaRotatingFrame(1) 0 ]; +initialVelocityFixedFrame = angularVelocityTensor * initialPosition + initialVelocity; +initialVelocityRotatingFrame = initialVelocity; + if navFrameRotating == 0 omegaCoriolisIMU = [0;0;0]; else @@ -58,7 +79,8 @@ end currentRotatingFrame = Pose3; % rotating frame initially coincides with fixed frame at t=0 currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0 -currentVelocityFixedGT = initialVelocity; +currentVelocityFixedGT = initialVelocityFixedFrame; +currentVelocityRotatingGT = initialVelocityRotatingFrame; % epsBias = 1e-20; sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); @@ -119,11 +141,13 @@ fprintf('Realistic Values = %d\n', useRealisticValues); fprintf('deltaT = %f\n', deltaT); fprintf('timeElapsed = %f\n', timeElapsed); fprintf('omegaRotatingFrame = [%f %f %f]\n', omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +fprintf('omegaCoriolisIMU = [%f %f %f]\n', omegaCoriolisIMU(1), omegaCoriolisIMU(2), omegaCoriolisIMU(3)); fprintf('omegaFixed = [%f %f %f]\n', omegaFixed(1), omegaFixed(2), omegaFixed(3)); fprintf('accelFixed = [%f %f %f]\n', accelFixed(1), accelFixed(2), accelFixed(3)); fprintf('Initial Velocity = [%f %f %f]\n', initialVelocity(1), initialVelocity(2), initialVelocity(3)); fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3)); fprintf('\n'); + %% Main loop: iterate through the ground truth trajectory, add factors % and values to the factor graph, and perform inference for i = 1:length(times) @@ -136,8 +160,13 @@ for i = 1:length(times) %% Set priors on the first iteration if(i == 1) - currentPoseEstimate = currentPoseFixedGT; % known initial conditions - currentVelocityEstimate = LieVector(currentVelocityFixedGT); % known initial conditions + % known initial conditions + currentPoseEstimate = currentPoseFixedGT; + if navFrameRotating == 1 + currentVelocityEstimate = LieVector(currentVelocityRotatingGT); + else + currentVelocityEstimate = LieVector(currentVelocityFixedGT); + end % Set Priors newValues.insert(currentPoseKey, currentPoseEstimate); @@ -264,16 +293,16 @@ for i = 1:length(times) %hold on; plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r'); hold on; - %plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'x'); - %plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'o'); + plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr'); + plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'or'); - plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), '-g'); - %plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); - %plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og'); + plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), 'g'); + plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); + plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og'); - plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), '-b'); - %plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); - %plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); + plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b'); + plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); + plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); hold off; xlabel('X axis') @@ -378,16 +407,16 @@ grid on; hold off; % TODO: logging rotation errors -for all time steps - Rerror = Rgt' * Restimated; - % transforming rotation matrix to axis-angle representation - vector_error = Rot3.Logmap(Rerror); - norm(vector_error) - - axis angle: [u,theta], with norm(u)=1 - vector_error = u * theta; +%for all time steps +% Rerror = Rgt' * Restimated; +% % transforming rotation matrix to axis-angle representation +% vector_error = Rot3.Logmap(Rerror); +% norm(vector_error) +% +% axis angle: [u,theta], with norm(u)=1 +% vector_error = u * theta; % TODO: logging velocity errors -velocities.. +%velocities.. From 1766b83adb05a0bd73933cefe89c494977cb62fb Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 13 Feb 2014 15:47:58 -0500 Subject: [PATCH 13/90] added rotation error --- .../+imuSimulator/coriolisExample.m | 44 +++++++++++++++---- 1 file changed, 36 insertions(+), 8 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index f783e29f7..f1e15581c 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -104,8 +104,9 @@ positionsInRotatingGT = zeros(3, length(times)); velocityInRotatingGT = zeros(3, length(times)); positionsEstimates = zeros(3,length(times)); -velocityEstimates = zeros(3,length(times)); -%rotationsEstimates = zeros(3,length(times)); +velocitiesEstimates = zeros(3,length(times)); + +rotationsErrorVectors = zeros(3,length(times)); % Rotating/Fixed frame selected later changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step h = figure(1); @@ -281,9 +282,18 @@ for i = 1:length(times) currentBias = isam.calculateEstimate(currentBiasKey); positionsEstimates(:,i) = currentPoseEstimate.translation.vector; - %rotationsEstimates(:,i) = currentPoseEstimate.rotation.vector; velocitiesEstimates(:,i) = currentVelocityEstimate.vector; biasEstimates(:,i) = currentBias.vector; + + % In matrix form: R_error = R_gt'*R_estimate + % Perform Logmap on the rotation matrix to get a vector + if navFrameRotating == 1 + rotationError = Rot3(currentPoseRotatingGT.rotation.matrix' * currentPoseEstimate.rotation.matrix); + else + rotationError = Rot3(currentPoseFixedGT.rotation.matrix' * currentPoseEstimate.rotation.matrix); + end + + rotationsErrorVectors(:,i) = Rot3.Logmap(rotationError); end end @@ -333,40 +343,57 @@ for i = 2:length(positionsEstimates) end %% Print and plot error results +% Calculate errors for appropriate navigation scheme if navFrameRotating == 0 axisPositionsError = positionsInFixedGT - positionsEstimates; - axisVelocityError = velocityInFixedGT - velocityEstimates; + axisVelocityError = velocityInFixedGT - velocitiesEstimates; else axisPositionsError = positionsInRotatingGT - positionsEstimates; - axisVelocityError = velocityInRotatingGT - velocityEstimates; + axisVelocityError = velocityInRotatingGT - velocitiesEstimates; end + +% Plot individual axis position errors figure plot(times, axisPositionsError); plotTitle = sprintf('Axis Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); title(plotTitle); -xlabel('Time'); +xlabel('Time [s]'); ylabel('Error (ground_truth - estimate) [m]'); legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); +% Plot 3D position error figure positionError3D = sqrt(axisPositionsError(1,:).^2+axisPositionsError(2,:).^2 + axisPositionsError(3,:).^2); plot(times, positionError3D); plotTitle = sprintf('3D Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); title(plotTitle); -xlabel('Time'); +xlabel('Time [s]'); ylabel('3D error [meters]'); +% Plot individual axis velocity errors figure plot(times, axisVelocityError); plotTitle = sprintf('Axis Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); title(plotTitle); -xlabel('Time'); +xlabel('Time [s]'); ylabel('Error (ground_truth - estimate) [m/s]'); legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); +% Plot magnitude of rotation errors +figure +for i = 1:size(rotationsErrorVectors,2) + rotationsErrorMagnitudes(i) = norm(rotationsErrorVectors(:,i)); +end +plot(times,rotationsErrorMagnitudes); +title('Rotation Error'); +xlabel('Time [s]'); +ylabel('Error [rads]'); + + +% Text output for errors if navFrameRotating == 0 fprintf('Fixed Frame ground truth trajectory length is %f [m]\n', trajectoryLengthFixedFrameGT); fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated); @@ -379,6 +406,7 @@ else norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthRotatingFrameGT*100); end fprintf('Final velocity error = [%f %f %f] [m/s]\n', axisVelocityError(1,end), axisVelocityError(2,end), axisVelocityError(3,end)); +fprintf('Final rotation error = %f [rads]\n', norm(rotationsErrorVectors(:,end))); %% Plot final trajectories figure From 8f4c3fd02d1e82930f52fd69f3cc9e169916dee4 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 13 Feb 2014 16:01:35 -0500 Subject: [PATCH 14/90] Fixed issue with velocity errors --- matlab/unstable_examples/+imuSimulator/coriolisExample.m | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index f1e15581c..6a19243da 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -17,7 +17,7 @@ import gtsam.*; addpath(genpath('./Libraries')) %% General configuration -navFrameRotating = 1; % 0 = perform navigation in the fixed frame +navFrameRotating = 0; % 0 = perform navigation in the fixed frame % 1 = perform navigation in the rotating frame IMU_type = 1; % IMU type 1 or type 2 useRealisticValues = 1; % use reaslist values for initial position and earth rotation @@ -184,6 +184,7 @@ for i = 1:length(times) positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; %velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity.vector; positionsEstimates(:,i) = currentPoseEstimate.translation.vector; + velocitiesEstimates(:,i) = currentVelocityEstimate.vector; currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector; currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix; else @@ -205,7 +206,7 @@ for i = 1:length(times) % Get velocity in rotating frame by treating it like a position and using compose % Maybe Luca knows a better way to do this within GTSAM. - currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT)); + currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT-initialVelocityFixedFrame)); currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT); % Store GT (ground truth) poses From b21a46c437383d38058120778d7156b8e2b328e0 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 13 Feb 2014 16:16:49 -0500 Subject: [PATCH 15/90] added velocity error figure --- .../+imuSimulator/coriolisExample.m | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 6a19243da..21c24c29e 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -17,7 +17,7 @@ import gtsam.*; addpath(genpath('./Libraries')) %% General configuration -navFrameRotating = 0; % 0 = perform navigation in the fixed frame +navFrameRotating = 1; % 0 = perform navigation in the fixed frame % 1 = perform navigation in the rotating frame IMU_type = 1; % IMU type 1 or type 2 useRealisticValues = 1; % use reaslist values for initial position and earth rotation @@ -383,6 +383,16 @@ xlabel('Time [s]'); ylabel('Error (ground_truth - estimate) [m/s]'); legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); +% Plot 3D velocity error +figure +velocityError3D = sqrt(axisVelocityError(1,:).^2+axisVelocityError(2,:).^2 + axisVelocityError(3,:).^2); +plot(times, velocityError3D); +plotTitle = sprintf('3D Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('3D error [meters/s]'); + % Plot magnitude of rotation errors figure for i = 1:size(rotationsErrorVectors,2) From a1e6d84e224d485f83429b0b8c4ef1c2bbaa0377 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 13 Feb 2014 17:32:03 -0500 Subject: [PATCH 16/90] added second order terms for Coriolis (still in progress) --- .cproject | 426 ++++++++++++++++++----------------- gtsam/navigation/ImuFactor.h | 15 +- 2 files changed, 233 insertions(+), 208 deletions(-) diff --git a/.cproject b/.cproject index 19005c63f..f4f6a288f 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -542,14 +540,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -576,6 +566,7 @@ make + tests/testBayesTree.run true false @@ -583,6 +574,7 @@ make + testBinaryBayesNet.run true false @@ -630,6 +622,7 @@ make + testSymbolicBayesNet.run true false @@ -637,6 +630,7 @@ make + tests/testSymbolicFactor.run true false @@ -644,6 +638,7 @@ make + testSymbolicFactorGraph.run true false @@ -659,11 +654,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -760,22 +764,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -792,6 +780,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -816,46 +820,6 @@ true true - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run - true - true - true - make -j5 @@ -920,6 +884,46 @@ true true + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j6 -j8 + testWhiteNoiseFactor.run + true + true + true + make -j2 @@ -1330,6 +1334,7 @@ make + testGraph.run true false @@ -1337,6 +1342,7 @@ make + testJunctionTree.run true false @@ -1344,6 +1350,7 @@ make + testSymbolicBayesNetB.run true false @@ -1511,6 +1518,7 @@ make + testErrors.run true false @@ -1556,22 +1564,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1652,6 +1644,22 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -2014,7 +2022,6 @@ make - testSimulated2DOriented.run true false @@ -2054,7 +2061,6 @@ make - testSimulated2D.run true false @@ -2062,7 +2068,6 @@ make - testSimulated3D.run true false @@ -2076,6 +2081,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j5 @@ -2350,7 +2371,6 @@ make - tests/testGaussianISAM2 true false @@ -2372,102 +2392,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2669,6 +2593,7 @@ cpack + -G DEB true false @@ -2676,6 +2601,7 @@ cpack + -G RPM true false @@ -2683,6 +2609,7 @@ cpack + -G TGZ true false @@ -2690,6 +2617,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2855,34 +2783,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2926,6 +2918,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 7438541d7..e619b559f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -411,12 +411,12 @@ namespace gtsam { Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), // dfP/dPi - - Rot_i.matrix(), + - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij, // dfV/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), // dfV/dPi - Matrix3::Zero(), + skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij, // dfR/dRi Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), // dfR/dPi @@ -431,10 +431,9 @@ namespace gtsam { + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(); - + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(); } if(H3) { @@ -487,13 +486,15 @@ namespace gtsam { + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) - vel_i * deltaTij + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * pos_i * deltaTij*deltaTij - 0.5 * gravity_ * deltaTij*deltaTij; const Vector3 fv = vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + + skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * pos_i * deltaTij // Coriolis term - gravity_ * deltaTij; const Vector3 fR = Rot3::Logmap(fRhat); From a01fe12ee68b77286c04890a64b33ea3c70abef9 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 13 Feb 2014 17:32:16 -0500 Subject: [PATCH 17/90] fixed velocity in rotating frame --- .../+imuSimulator/coriolisExample.m | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 21c24c29e..1613c4315 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -200,20 +200,27 @@ for i = 1:length(times) currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation % Rotate pose in fixed frame to get pose in rotating frame + previousPositionRotatingGT = currentPoseRotatingGT.translation.vector; currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame); inverseCurrentRotatingFrame = (currentRotatingFrame.inverse); currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT); + currentPositionRotatingGT = currentPoseRotatingGT.translation.vector; % Get velocity in rotating frame by treating it like a position and using compose % Maybe Luca knows a better way to do this within GTSAM. - currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT-initialVelocityFixedFrame)); - currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT); + %currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT-initialVelocityFixedFrame)); + %currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT); + + %currentRot3RotatingGT = currentRotatingFrame.rotation(); + %currentVelocityRotatingGT = currentRot3RotatingGT.unrotate(Point3(currentVelocityFixedGT)); + % TODO: check if initial velocity in rotating frame is correct + currentVelocityRotatingGT = (currentPositionRotatingGT-previousPositionRotatingGT)/deltaT; % Store GT (ground truth) poses positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector; velocityInFixedGT(:,i) = currentVelocityFixedGT; positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; - velocityInRotatingGT(:,i) = currentVelocityPoseRotatingGT.translation.vector; + velocityInRotatingGT(:,i) = currentVelocityRotatingGT; currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector; currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix; @@ -265,6 +272,7 @@ for i = 1:length(times) % Add values to the graph. Use the current pose and velocity % estimates as to values when interpreting the next pose and % velocity estimates + %ImuFactor.Predict(currentPoseEstimate, currentVelocityEstimate, pose_j, vel_j, zeroBias, currentSummarizedMeasurement, g, omegaCoriolisIMU); newValues.insert(currentPoseKey, currentPoseEstimate); newValues.insert(currentVelKey, currentVelocityEstimate); newValues.insert(currentBiasKey, zeroBias); From e65d075c205ae3082fc58d8209dcaed1147a9cea Mon Sep 17 00:00:00 2001 From: djensen3 Date: Fri, 14 Feb 2014 10:22:21 -0500 Subject: [PATCH 18/90] added option for external initial conditions --- .../+imuSimulator/coriolisExample.m | 96 ++++++++++--------- 1 file changed, 51 insertions(+), 45 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 1613c4315..ad892c7e0 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -8,55 +8,56 @@ % Its position is plotted in both the fixed and rotating reference frames to simulate how an observer in each frame would % experience the body's motion. -clc -clear all -close all - import gtsam.*; addpath(genpath('./Libraries')) -%% General configuration -navFrameRotating = 1; % 0 = perform navigation in the fixed frame - % 1 = perform navigation in the rotating frame -IMU_type = 1; % IMU type 1 or type 2 -useRealisticValues = 1; % use reaslist values for initial position and earth rotation -record_movie = 0; % 0 = do not record movie - % 1 = record movie of the trajectories. One - % frame per time step (15 fps) +% Check for an external configuarion. This is useful for setting up +% multiple tests +if ~exist('externalCoriolisConfiguration', 'var') + clc + clear all + close all + %% General configuration + navFrameRotating = 1; % 0 = perform navigation in the fixed frame + % 1 = perform navigation in the rotating frame + IMU_type = 1; % IMU type 1 or type 2 + useRealisticValues = 1; % use reaslist values for initial position and earth rotation + record_movie = 0; % 0 = do not record movie + % 1 = record movie of the trajectories. One + % frame per time step (15 fps -%% Scenario Configuration -deltaT = 0.01; % timestep -timeElapsed = 5; % Total elapsed time -times = 0:deltaT:timeElapsed; + %% Scenario Configuration + deltaT = 0.01; % timestep + timeElapsed = 5; % Total elapsed time + times = 0:deltaT:timeElapsed; -% Initial Conditions -omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s) -radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km -angularVelocityTensorEarth = [ 0 -omegaEarthSeconds(3) omegaEarthSeconds(2); - omegaEarthSeconds(3) 0 -omegaEarthSeconds(1); - -omegaEarthSeconds(2) omegaEarthSeconds(1) 0 ]; -if useRealisticValues == 1 - omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame - omegaFixed = [0;0;0]; % constant rotation rate measurement - accelFixed = [0.5;-0.5;0]; % constant acceleration measurement - g = [0;0;0]; % Gravity - initialLongitude = 45; - initialLatitude = 30; - % initial position at some [longitude, latitude] location on earth's - % surface (approximating Earth as a sphere) - initialPosition = [radiusEarth*sind(initialLongitude); - radiusEarth*cosd(initialLongitude); - radiusEarth*sind(initialLatitude)]; - initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, - % (ignoring the velocity due to the earth's rotation) -else - omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame - omegaFixed = [0;0;0]; % constant rotation rate measurement - accelFixed = [0.1;0;0]; % constant acceleration measurement - g = [0;0;0]; % Gravity - initialPosition = [0; 1; 0];% initial position in both frames - initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) + % Initial Conditions + omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s) + radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km + + if useRealisticValues == 1 + omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [-0.5;0.5;2]; % constant acceleration measurement + g = [0;0;0]; % Gravity + initialLongitude = 45; % longitude in degrees + initialLatitude = 30; % latitude in degrees + % initial position at some [longitude, latitude] location on earth's + % surface (approximating Earth as a sphere) + initialPosition = [radiusEarth*sind(initialLongitude); + radiusEarth*cosd(initialLongitude); + radiusEarth*sind(initialLatitude)]; + initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, + % (ignoring the velocity due to the earth's rotation) + else + omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [0.1;0;0]; % constant acceleration measurement + g = [0;0;0]; % Gravity + initialPosition = [0; 1; 0];% initial position in both frames + initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) + end end % From Wikipedia Angular Velocity page, dr/dt = W*r, where r is @@ -109,7 +110,7 @@ velocitiesEstimates = zeros(3,length(times)); rotationsErrorVectors = zeros(3,length(times)); % Rotating/Fixed frame selected later changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step -h = figure(1); +h = figure; % Solver object isamParams = ISAM2Params; @@ -146,7 +147,12 @@ fprintf('omegaCoriolisIMU = [%f %f %f]\n', omegaCoriolisIMU(1), omegaCoriolisIMU fprintf('omegaFixed = [%f %f %f]\n', omegaFixed(1), omegaFixed(2), omegaFixed(3)); fprintf('accelFixed = [%f %f %f]\n', accelFixed(1), accelFixed(2), accelFixed(3)); fprintf('Initial Velocity = [%f %f %f]\n', initialVelocity(1), initialVelocity(2), initialVelocity(3)); -fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3)); +if(exist('initialLatitude', 'var') && exist('initialLongitude', 'var')) + fprintf('Initial Position\n\t[Long, Lat] = [%f %f] degrees\n\tEFEC = [%f %f %f]\n', ... + initialLongitude, initialLatitude, initialPosition(1), initialPosition(2), initialPosition(3)); +else + fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3)); +end fprintf('\n'); %% Main loop: iterate through the ground truth trajectory, add factors From 4ef8cec118160edfe667cc3170283890524ea587 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 14 Feb 2014 17:41:58 -0500 Subject: [PATCH 19/90] added monte carlo runs --- .../+imuSimulator/coriolisExample.m | 55 ++++++++++----- .../+imuSimulator/coriolisTestMonteCarlo.m | 68 +++++++++++++++++++ 2 files changed, 106 insertions(+), 17 deletions(-) create mode 100644 matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index ad892c7e0..e128dacb4 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -12,6 +12,8 @@ import gtsam.*; addpath(genpath('./Libraries')) +estimationEnabled = 1 + % Check for an external configuarion. This is useful for setting up % multiple tests if ~exist('externalCoriolisConfiguration', 'var') @@ -53,7 +55,7 @@ if ~exist('externalCoriolisConfiguration', 'var') else omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame omegaFixed = [0;0;0]; % constant rotation rate measurement - accelFixed = [0.1;0;0]; % constant acceleration measurement + accelFixed = [1;0;0]; % constant acceleration measurement g = [0;0;0]; % Gravity initialPosition = [0; 1; 0];% initial position in both frames initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) @@ -286,7 +288,7 @@ for i = 1:length(times) %newFactors.print(''); newValues.print(''); %% Solve factor graph - if(i > 1) + if(i > 1 && estimationEnabled) isam.update(newFactors, newValues); newFactors = NonlinearFactorGraph; newValues = Values; @@ -325,9 +327,11 @@ for i = 1:length(times) plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og'); - plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b'); - plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); - plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); + if(estimationEnabled) + plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b'); + plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); + plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); + end hold off; xlabel('X axis') @@ -387,6 +391,21 @@ title(plotTitle); xlabel('Time [s]'); ylabel('3D error [meters]'); +% Plot 3D position error +if navFrameRotating == 0 + trajLen = trajectoryLengthFixedFrameGT; +else + trajLen = trajectoryLengthRotatingFrameGT; +end + +figure +plot(times, (positionError3D/trajLen)*100); +plotTitle = sprintf('3D Error normalized by distance in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('normalized 3D error [% of distance traveled]'); + % Plot individual axis velocity errors figure plot(times, axisVelocityError); @@ -435,22 +454,26 @@ fprintf('Final rotation error = %f [rads]\n', norm(rotationsErrorVectors(:,end)) %% Plot final trajectories figure -sphere % sphere for reference +[x,y,z] = sphere(30); +mesh(radiusEarth*x,radiusEarth*y, radiusEarth*z) % where (a,b,c) is center of the sphere % sphere for reference hold on; +axis equal % Ground truth trajectory in fixed reference frame -plot3(positionsInFixedGT(1,:), positionsInFixedGT(2,:), positionsInFixedGT(3,:),'r'); -plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr'); -plot3(positionsInFixedGT(1,end), positionsInFixedGT(2,end), positionsInFixedGT(3,end), 'or'); +plot3(positionsInFixedGT(1,:), positionsInFixedGT(2,:), positionsInFixedGT(3,:),'r','lineWidth',4); +plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr','lineWidth',4); +plot3(positionsInFixedGT(1,end), positionsInFixedGT(2,end), positionsInFixedGT(3,end), 'or','lineWidth',4); % Ground truth trajectory in rotating reference frame -plot3(positionsInRotatingGT(1,:), positionsInRotatingGT(2,:), positionsInRotatingGT(3,:), '-g'); -plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); -plot3(positionsInRotatingGT(1,end), positionsInRotatingGT(2,end), positionsInRotatingGT(3,end), 'og'); +plot3(positionsInRotatingGT(1,:), positionsInRotatingGT(2,:), positionsInRotatingGT(3,:), '-g','lineWidth',4); +plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg','lineWidth',4); +plot3(positionsInRotatingGT(1,end), positionsInRotatingGT(2,end), positionsInRotatingGT(3,end), 'og','lineWidth',4); % Estimates -plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-b'); -plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); -plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'ob'); +if(estimationEnabled) + plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-b','lineWidth',4); + plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb','lineWidth',4); + plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'ob','lineWidth',4); +end xlabel('X axis') ylabel('Y axis') @@ -471,5 +494,3 @@ hold off; % TODO: logging velocity errors %velocities.. - - diff --git a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m new file mode 100644 index 000000000..9c66ba134 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m @@ -0,0 +1,68 @@ +clc +clear all +close all + +% Flag to mark external configuration +externalCoriolisConfiguration = 1; + +%% General configuration +navFrameRotating = 0; % 0 = perform navigation in the fixed frame + % 1 = perform navigation in the rotating frame +IMU_type = 1; % IMU type 1 or type 2 +useRealisticValues = 1; % use reaslist values for initial position and earth rotation +record_movie = 0; % 0 = do not record movie + % 1 = record movie of the trajectories. One + % frame per time step (15 fps) + +%% Scenario Configuration +deltaT = 0.01; % timestep +timeElapsed = 5; % Total elapsed time +times = 0:deltaT:timeElapsed; + +% Initial Conditions +omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s) +radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km + +if useRealisticValues == 1 + omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [0.1;0;1]; % constant acceleration measurement + g = [0;0;0]; % Gravity + initialLongitude = -45; % longitude in degrees + initialLatitude = 30; % latitude in degrees + % initial position at some [longitude, latitude] location on earth's + % surface (approximating Earth as a sphere) + initialPosition = [radiusEarth*sind(initialLongitude); + radiusEarth*cosd(initialLongitude); + radiusEarth*sind(initialLatitude)]; + initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, + % (ignoring the velocity due to the earth's rotation) +else + omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [1;0;0]; % constant acceleration measurement + g = [0;0;0]; % Gravity + initialPosition = [0; 1; 0];% initial position in both frames + initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) +end + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +numTests = 10 +% for testInd=1:numTests +% accelFixed = 2*rand(3,1)-ones(3,1); +% imuSimulator.coriolisExample +% posFinError(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100 +% rotFinError(testInd) = norm(rotationsErrorVectors(:,end)) +% velFinError(testInd) = norm(axisVelocityError(:,end)) +% end + +% Run the same initial conditions but navigating in the rotating frame +navFrameRotating = 1; +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +for testInd=1:numTests + accelFixed = 2*rand(3,1)-ones(3,1); + imuSimulator.coriolisExample + posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100 + rotFinErrorRot(testInd) = norm(rotationsErrorVectors(:,end)) + velFinErrorRot(testInd) = norm(axisVelocityError(:,end)) +end \ No newline at end of file From c8c728ad6f603e8be6182f9e107394fbfa8aa16d Mon Sep 17 00:00:00 2001 From: djensen Date: Mon, 17 Feb 2014 09:36:37 -0500 Subject: [PATCH 20/90] added additional coriolis terms --- gtsam/navigation/CombinedImuFactor.h | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index c807fd356..11d154c91 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -452,12 +452,12 @@ namespace gtsam { Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), // dfP/dPi - - Rot_i.matrix(), + - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij, // dfV/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), // dfV/dPi - Matrix3::Zero(), + skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij, // dfR/dRi Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), // dfR/dPi @@ -476,14 +476,13 @@ namespace gtsam { + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); - + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(), + //dBiasAcc/dVi + Matrix3::Zero(), + //dBiasOmega/dVi + Matrix3::Zero(); } if(H3) { @@ -564,6 +563,7 @@ namespace gtsam { + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) - vel_i * deltaTij + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * pos_i * deltaTij*deltaTij - 0.5 * gravity_ * deltaTij*deltaTij; const Vector3 fv = @@ -571,6 +571,7 @@ namespace gtsam { + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term + + skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * pos_i * deltaTij // Coriolis term - gravity_ * deltaTij; const Vector3 fR = Rot3::Logmap(fRhat); From 794a22249bde7b68b61e4cf7c6c39295d72354b5 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 17 Feb 2014 10:13:13 -0500 Subject: [PATCH 21/90] small change --- .../+imuSimulator/coriolisTestMonteCarlo.m | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m index 9c66ba134..eaec0f8c3 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m @@ -48,21 +48,19 @@ end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% numTests = 10 -% for testInd=1:numTests -% accelFixed = 2*rand(3,1)-ones(3,1); -% imuSimulator.coriolisExample -% posFinError(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100 -% rotFinError(testInd) = norm(rotationsErrorVectors(:,end)) -% velFinError(testInd) = norm(axisVelocityError(:,end)) -% end - -% Run the same initial conditions but navigating in the rotating frame -navFrameRotating = 1; -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for testInd=1:numTests + navFrameRotating = 0; accelFixed = 2*rand(3,1)-ones(3,1); imuSimulator.coriolisExample + posFinError(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100 + rotFinError(testInd) = norm(rotationsErrorVectors(:,end)) + velFinError(testInd) = norm(axisVelocityError(:,end)) + % Run the same initial conditions but navigating in the rotating frame + navFrameRotating = 1; + imuSimulator.coriolisExample posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100 rotFinErrorRot(testInd) = norm(rotationsErrorVectors(:,end)) velFinErrorRot(testInd) = norm(axisVelocityError(:,end)) -end \ No newline at end of file +end + + From 8f397345801a01d93b6cc552ae8740e89caa4893 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 17 Feb 2014 10:34:02 -0500 Subject: [PATCH 22/90] extra lines.. to be completed --- .../+imuSimulator/coriolisTestMonteCarlo.m | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m index eaec0f8c3..f525a407f 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m @@ -47,14 +47,14 @@ else end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -numTests = 10 +numTests = 20 for testInd=1:numTests navFrameRotating = 0; accelFixed = 2*rand(3,1)-ones(3,1); imuSimulator.coriolisExample - posFinError(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100 - rotFinError(testInd) = norm(rotationsErrorVectors(:,end)) - velFinError(testInd) = norm(axisVelocityError(:,end)) + posFinErrorFixed(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100 + rotFinErrorFixed(testInd) = norm(rotationsErrorVectors(:,end)) + velFinErrorFixed(testInd) = norm(axisVelocityError(:,end)) % Run the same initial conditions but navigating in the rotating frame navFrameRotating = 1; imuSimulator.coriolisExample @@ -63,4 +63,9 @@ for testInd=1:numTests velFinErrorRot(testInd) = norm(axisVelocityError(:,end)) end +mean(posFinErrorFixed) +max(posFinErrorFixed) + +box(posFinErrorFixed) + From 3a7a636f0fec52dd39799ca00fe1c62910f01155 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 17 Feb 2014 10:41:42 -0500 Subject: [PATCH 23/90] minor --- .../+imuSimulator/coriolisExample.m | 12 ++++++------ .../+imuSimulator/coriolisTestMonteCarlo.m | 11 +++++++++++ 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index e128dacb4..555ca553e 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -60,6 +60,12 @@ if ~exist('externalCoriolisConfiguration', 'var') initialPosition = [0; 1; 0];% initial position in both frames initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) end + + if navFrameRotating == 0 + omegaCoriolisIMU = [0;0;0]; + else + omegaCoriolisIMU = omegaRotatingFrame; + end end % From Wikipedia Angular Velocity page, dr/dt = W*r, where r is @@ -72,12 +78,6 @@ angularVelocityTensor = [ 0 -omegaRotatingFrame(3) omegaRot initialVelocityFixedFrame = angularVelocityTensor * initialPosition + initialVelocity; initialVelocityRotatingFrame = initialVelocity; -if navFrameRotating == 0 - omegaCoriolisIMU = [0;0;0]; -else - omegaCoriolisIMU = omegaRotatingFrame; -end - % currentRotatingFrame = Pose3; % rotating frame initially coincides with fixed frame at t=0 currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); diff --git a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m index f525a407f..d48abca2c 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m @@ -49,13 +49,24 @@ end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% numTests = 20 for testInd=1:numTests + omegaCoriolisIMU = [0;0;0]; navFrameRotating = 0; accelFixed = 2*rand(3,1)-ones(3,1); imuSimulator.coriolisExample posFinErrorFixed(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100 rotFinErrorFixed(testInd) = norm(rotationsErrorVectors(:,end)) velFinErrorFixed(testInd) = norm(axisVelocityError(:,end)) + % Run the same initial conditions but navigating in the rotating frame +--> enable coriolis effect by setting:omegaCoriolisIMU = omegaRotatingFrame; + navFrameRotating = 1; + imuSimulator.coriolisExample + posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100 + rotFinErrorRot(testInd) = norm(rotationsErrorVectors(:,end)) + velFinErrorRot(testInd) = norm(axisVelocityError(:,end)) + + % Run the same initial conditions but navigating in the rotating frame +--> disable coriolis effect by setting: omegaCoriolisIMU = [0;0;0]; navFrameRotating = 1; imuSimulator.coriolisExample posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100 From dfcd2cb3ba6230943b5bdf681f3b8452df827fa9 Mon Sep 17 00:00:00 2001 From: djensen Date: Wed, 19 Feb 2014 16:57:15 -0500 Subject: [PATCH 24/90] added random initial positions to monte carlo tests --- .../+imuSimulator/coriolisExample.m | 97 ++++++------ .../+imuSimulator/coriolisTestMonteCarlo.m | 141 +++++++++++++----- 2 files changed, 162 insertions(+), 76 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 555ca553e..7e3719143 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -12,8 +12,6 @@ import gtsam.*; addpath(genpath('./Libraries')) -estimationEnabled = 1 - % Check for an external configuarion. This is useful for setting up % multiple tests if ~exist('externalCoriolisConfiguration', 'var') @@ -21,13 +19,16 @@ if ~exist('externalCoriolisConfiguration', 'var') clear all close all %% General configuration - navFrameRotating = 1; % 0 = perform navigation in the fixed frame + navFrameRotating = 0; % 0 = perform navigation in the fixed frame % 1 = perform navigation in the rotating frame IMU_type = 1; % IMU type 1 or type 2 useRealisticValues = 1; % use reaslist values for initial position and earth rotation record_movie = 0; % 0 = do not record movie % 1 = record movie of the trajectories. One - % frame per time step (15 fps + % frame per time step (15 fps) + incrementalPlotting = 0; % turn incremental plotting on and off. Turning plotting off increases + % speed for batch testing + estimationEnabled = 1; %% Scenario Configuration deltaT = 0.01; % timestep @@ -43,21 +44,21 @@ if ~exist('externalCoriolisConfiguration', 'var') omegaFixed = [0;0;0]; % constant rotation rate measurement accelFixed = [-0.5;0.5;2]; % constant acceleration measurement g = [0;0;0]; % Gravity - initialLongitude = 45; % longitude in degrees - initialLatitude = 30; % latitude in degrees % initial position at some [longitude, latitude] location on earth's % surface (approximating Earth as a sphere) - initialPosition = [radiusEarth*sind(initialLongitude); - radiusEarth*cosd(initialLongitude); - radiusEarth*sind(initialLatitude)]; + initialLongitude = 45; % longitude in degrees + initialLatitude = 30; % latitude in degrees + initialAltitude = 0; % Altitude above Earth's surface in meters + [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude); + initialPosition = [x; y; z]; initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, % (ignoring the velocity due to the earth's rotation) else - omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame + omegaRotatingFrame = [0;0;pi/5]; % rotation of the moving frame wrt fixed frame omegaFixed = [0;0;0]; % constant rotation rate measurement - accelFixed = [1;0;0]; % constant acceleration measurement + accelFixed = [0.5;0.5;0.5]; % constant acceleration measurement g = [0;0;0]; % Gravity - initialPosition = [0; 1; 0];% initial position in both frames + initialPosition = [1; 0; 0];% initial position in both frames initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) end @@ -68,6 +69,8 @@ if ~exist('externalCoriolisConfiguration', 'var') end end + + % From Wikipedia Angular Velocity page, dr/dt = W*r, where r is % position vector and W is angular velocity tensor % We add the initial velocity in the rotating frame because they @@ -139,6 +142,7 @@ if navFrameRotating == 0 else fprintf('Performing navigation in the Rotating frame.\n'); end +fprintf('Estimation Enabled = %d\n', estimationEnabled); fprintf('IMU_type = %d\n', IMU_type); fprintf('Record Movie = %d\n', record_movie); fprintf('Realistic Values = %d\n', useRealisticValues); @@ -223,6 +227,8 @@ for i = 1:length(times) %currentVelocityRotatingGT = currentRot3RotatingGT.unrotate(Point3(currentVelocityFixedGT)); % TODO: check if initial velocity in rotating frame is correct currentVelocityRotatingGT = (currentPositionRotatingGT-previousPositionRotatingGT)/deltaT; + %currentVelocityRotatingGT = (currentPositionRotatingGT - previousPositionRotatingGT ... + % - 0.5 * accelFixed * deltaT * deltaT) / deltaT + accelFixed * deltaT; % Store GT (ground truth) poses positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector; @@ -315,35 +321,38 @@ for i = 1:length(times) end %% incremental plotting for animation (ground truth) - figure(h) - %plot_trajectory(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1) - %hold on; - plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r'); - hold on; - plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr'); - plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'or'); - - plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), 'g'); - plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); - plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og'); - - if(estimationEnabled) - plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b'); - plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); - plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); - end - - hold off; - xlabel('X axis') - ylabel('Y axis') - zlabel('Z axis') - axis equal - grid on; - %pause(0.1); - - if record_movie == 1 - frame = getframe(gcf); - writeVideo(writerObj,frame); + if incrementalPlotting == 1 + figure(h) + %plot_trajectory(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1) + %hold on; + plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r'); + hold on; + plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr'); + plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'or'); + + plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), 'g'); + plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); + plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og'); + + if(estimationEnabled) + plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b'); + plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); + plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); + end + + hold off; + xlabel('X axis') + ylabel('Y axis') + zlabel('Z axis') + axis equal + grid on; + %pause(0.1); + + % record frames for movie + if record_movie == 1 + frame = getframe(gcf); + writeVideo(writerObj,frame); + end end end @@ -455,7 +464,11 @@ fprintf('Final rotation error = %f [rads]\n', norm(rotationsErrorVectors(:,end)) %% Plot final trajectories figure [x,y,z] = sphere(30); -mesh(radiusEarth*x,radiusEarth*y, radiusEarth*z) % where (a,b,c) is center of the sphere % sphere for reference +if useRealisticValues + mesh(radiusEarth*x,radiusEarth*y, radiusEarth*z) % where (a,b,c) is center of the sphere % sphere for reference +else + mesh(x,y,z); +end hold on; axis equal % Ground truth trajectory in fixed reference frame diff --git a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m index d48abca2c..0a26bd760 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m @@ -13,6 +13,8 @@ useRealisticValues = 1; % use reaslist values for initial position and earth record_movie = 0; % 0 = do not record movie % 1 = record movie of the trajectories. One % frame per time step (15 fps) +incrementalPlotting = 0; +estimationEnabled = 1; %% Scenario Configuration deltaT = 0.01; % timestep @@ -28,15 +30,16 @@ if useRealisticValues == 1 omegaFixed = [0;0;0]; % constant rotation rate measurement accelFixed = [0.1;0;1]; % constant acceleration measurement g = [0;0;0]; % Gravity - initialLongitude = -45; % longitude in degrees + + initialLongitude = 45; % longitude in degrees initialLatitude = 30; % latitude in degrees - % initial position at some [longitude, latitude] location on earth's - % surface (approximating Earth as a sphere) - initialPosition = [radiusEarth*sind(initialLongitude); - radiusEarth*cosd(initialLongitude); - radiusEarth*sind(initialLatitude)]; + initialAltitude = 0; % Altitude above Earth's surface in meters + [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude); + initialPosition = [x; y; z]; + initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, % (ignoring the velocity due to the earth's rotation) + else omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame omegaFixed = [0;0;0]; % constant rotation rate measurement @@ -47,36 +50,106 @@ else end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -numTests = 20 -for testInd=1:numTests - omegaCoriolisIMU = [0;0;0]; - navFrameRotating = 0; - accelFixed = 2*rand(3,1)-ones(3,1); - imuSimulator.coriolisExample - posFinErrorFixed(testInd) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100 - rotFinErrorFixed(testInd) = norm(rotationsErrorVectors(:,end)) - velFinErrorFixed(testInd) = norm(axisVelocityError(:,end)) - - % Run the same initial conditions but navigating in the rotating frame ---> enable coriolis effect by setting:omegaCoriolisIMU = omegaRotatingFrame; - navFrameRotating = 1; - imuSimulator.coriolisExample - posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100 - rotFinErrorRot(testInd) = norm(rotationsErrorVectors(:,end)) - velFinErrorRot(testInd) = norm(axisVelocityError(:,end)) - - % Run the same initial conditions but navigating in the rotating frame ---> disable coriolis effect by setting: omegaCoriolisIMU = [0;0;0]; - navFrameRotating = 1; - imuSimulator.coriolisExample - posFinErrorRot(testInd) = norm(axisPositionsError(:,end))/trajLen*100 - rotFinErrorRot(testInd) = norm(rotationsErrorVectors(:,end)) - velFinErrorRot(testInd) = norm(axisVelocityError(:,end)) + +% Run tests with randomly generated initialPosition and accelFixed values +% For each initial position, a number of acceleration vectors are +% generated. For each (initialPosition, accelFixed) pair, the coriolis test +% is run for the following 3 scenarios +% - Navigation performed in fixed frame +% - Navigation performed in rotating frame, including the coriolis effect +% - Navigation performed in rotating frame, ignoring coriolis effect +% + +%% Testing configuration +numPosTests = 10; +numAccelTests = 20; +totalNumTests = numPosTests * numAccelTests; + +% Storage variables +posFinErrorFixed = zeros(numPosTests, numAccelTests); +rotFinErrorFixed = zeros(numPosTests, numAccelTests); +velFinErrorFixed = zeros(numPosTests, numAccelTests); + +posFinErrorRotCoriolis = zeros(numPosTests, numAccelTests); +rotFinErrorRotCoriolis = zeros(numPosTests, numAccelTests); +velFinErrorRotCoriolis = zeros(numPosTests, numAccelTests); + +posFinErrorRot = zeros(numPosTests, numAccelTests); +rotFinErrorRot = zeros(numPosTests, numAccelTests); +velFinErrorRot = zeros(numPosTests, numAccelTests); + + +numErrors = 0; +testIndPos = 1; +testIndAccel = 1; +while testIndPos <= numPosTests + %generate a random initial position vector + initialLongitude = rand()*360 - 180; % longitude in degrees (-90 to 90) + initialLatitude = rand()*180 - 90; % latitude in degrees (-180 to 180) + initialAltitude = rand()*150; % Altitude above Earth's surface in meters (0-150) + [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude); + initialPosition = [x; y; z]; + + while testIndAccel <= numAccelTests + [testIndPos testIndAccel] + % generate a random acceleration vector + accelFixed = 2*rand(3,1)-ones(3,1); + + try + omegaCoriolisIMU = [0;0;0]; + navFrameRotating = 0; + imuSimulator.coriolisExample + posFinErrorFixed(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100; + rotFinErrorFixed(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); + velFinErrorFixed(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + + close all; + + % Run the same initial conditions but navigating in the rotating frame + % enable coriolis effect by setting: + omegaCoriolisIMU = omegaRotatingFrame; + navFrameRotating = 1; + imuSimulator.coriolisExample + posFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; + rotFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); + velFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + + close all; + + % Run the same initial conditions but navigating in the rotating frame + % disable coriolis effect by setting: + omegaCoriolisIMU = [0;0;0]; + navFrameRotating = 1; + imuSimulator.coriolisExample + posFinErrorRot(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; + rotFinErrorRot(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); + velFinErrorRot(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + + close all; + catch + numErrors = numErrors + 1; + fprintf('*ERROR*'); + fprintf('%d tests cancelled due to errors\n', numErrors); + fprintf('restarting test with new accelFixed'); + testIndAccel = testIndAccel - 1; + end + testIndAccel = testIndAccel + 1; + end + testIndAccel = 1; + testIndPos = testIndPos + 1; end +fprintf('\nTotal: %d tests cancelled due to errors\n', numErrors); -mean(posFinErrorFixed) -max(posFinErrorFixed) +mean(posFinErrorFixed); +max(posFinErrorFixed); +% box(posFinErrorFixed); -box(posFinErrorFixed) +mean(posFinErrorRotCoriolis); +max(posFinErrorRotCoriolis); +% box(posFinErrorRotCoriolis); + +mean(posFinErrorRot); +max(posFinErrorRot); +% box(posFinErrorRot); From c623dafecfc3f0b23e90d1358d5d866a7dcd066b Mon Sep 17 00:00:00 2001 From: djensen Date: Thu, 20 Feb 2014 11:30:02 -0500 Subject: [PATCH 25/90] added second order coriolis term to predict() for IMU types 1 and 2 --- gtsam/navigation/CombinedImuFactor.h | 2 ++ gtsam/navigation/ImuFactor.h | 6 ++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 11d154c91..69b6c5b0a 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -607,12 +607,14 @@ namespace gtsam { + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + vel_i * deltaTij - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij // 2nd order coriolis term + 0.5 * gravity * deltaTij*deltaTij; vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij // 2nd order Coriolis term + gravity * deltaTij); const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index e619b559f..77a4084dc 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -486,7 +486,7 @@ namespace gtsam { + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr) - vel_i * deltaTij + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * pos_i * deltaTij*deltaTij + + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * pos_i * deltaTij*deltaTij // 2nd order coriolis term - 0.5 * gravity_ * deltaTij*deltaTij; const Vector3 fv = @@ -494,7 +494,7 @@ namespace gtsam { + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr) + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - + skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * pos_i * deltaTij // Coriolis term + + skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * pos_i * deltaTij // 2nd order Coriolis term - gravity_ * deltaTij; const Vector3 fR = Rot3::Logmap(fRhat); @@ -524,12 +524,14 @@ namespace gtsam { + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + vel_i * deltaTij - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij // 2nd order coriolis term + 0.5 * gravity * deltaTij*deltaTij; vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij // 2nd order Coriolis term + gravity * deltaTij); const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); From 98712b21c9581fbbe36750cc3f8787894426dbfd Mon Sep 17 00:00:00 2001 From: djensen Date: Tue, 25 Feb 2014 14:12:24 -0500 Subject: [PATCH 26/90] additional info saved for monte carlo tests --- .../+imuSimulator/coriolisExample.m | 5 ++- .../+imuSimulator/coriolisTestMonteCarlo.m | 35 ++++++++++++++++--- 2 files changed, 33 insertions(+), 7 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index 7e3719143..35d27aa73 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -19,14 +19,14 @@ if ~exist('externalCoriolisConfiguration', 'var') clear all close all %% General configuration - navFrameRotating = 0; % 0 = perform navigation in the fixed frame + navFrameRotating = 1; % 0 = perform navigation in the fixed frame % 1 = perform navigation in the rotating frame IMU_type = 1; % IMU type 1 or type 2 useRealisticValues = 1; % use reaslist values for initial position and earth rotation record_movie = 0; % 0 = do not record movie % 1 = record movie of the trajectories. One % frame per time step (15 fps) - incrementalPlotting = 0; % turn incremental plotting on and off. Turning plotting off increases + incrementalPlotting = 1; % turn incremental plotting on and off. Turning plotting off increases % speed for batch testing estimationEnabled = 1; @@ -70,7 +70,6 @@ if ~exist('externalCoriolisConfiguration', 'var') end - % From Wikipedia Angular Velocity page, dr/dt = W*r, where r is % position vector and W is angular velocity tensor % We add the initial velocity in the rotating frame because they diff --git a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m index 0a26bd760..4d02fd03e 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m @@ -2,7 +2,7 @@ clc clear all close all -% Flag to mark external configuration +% Flag to mark external configuration to the main test script externalCoriolisConfiguration = 1; %% General configuration @@ -49,6 +49,7 @@ else initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) end +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Run tests with randomly generated initialPosition and accelFixed values @@ -60,12 +61,13 @@ end % - Navigation performed in rotating frame, ignoring coriolis effect % -%% Testing configuration -numPosTests = 10; -numAccelTests = 20; +% Testing configuration +numPosTests = 1; +numAccelTests = 10; totalNumTests = numPosTests * numAccelTests; % Storage variables + posFinErrorFixed = zeros(numPosTests, numAccelTests); rotFinErrorFixed = zeros(numPosTests, numAccelTests); velFinErrorFixed = zeros(numPosTests, numAccelTests); @@ -82,6 +84,7 @@ velFinErrorRot = zeros(numPosTests, numAccelTests); numErrors = 0; testIndPos = 1; testIndAccel = 1; + while testIndPos <= numPosTests %generate a random initial position vector initialLongitude = rand()*360 - 180; % longitude in degrees (-90 to 90) @@ -95,6 +98,16 @@ while testIndPos <= numPosTests % generate a random acceleration vector accelFixed = 2*rand(3,1)-ones(3,1); + %lla = oldTestInfo(testIndPos,testIndAccel).initialPositionLLA; + %initialLongitude = lla(1); + %initialLatitude = lla(2); + %initialAltitude = lla(3); + %initialPosition = oldTestInfo(testIndPos, testIndAccel).initialPositionECEF; + + testInfo(testIndPos, testIndAccel).initialPositionLLA = [initialLongitude, initialLatitude, initialAltitude]; + testInfo(testIndPos, testIndAccel).initialPositionECEF = initialPosition; + testInfo(testIndPos, testIndAccel).accelFixed = accelFixed; + try omegaCoriolisIMU = [0;0;0]; navFrameRotating = 0; @@ -102,6 +115,12 @@ while testIndPos <= numPosTests posFinErrorFixed(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100; rotFinErrorFixed(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); velFinErrorFixed(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + testInfo(testIndPos, testIndAccel).fixedPositionError = axisPositionsError(:,end); + testInfo(testIndPos, testIndAccel).fixedVelocityError = axisVelocityError(:,end); + testInfo(testIndPos, testIndAccel).fixedRotationError = rotationsErrorVectors(:,end); + testInfo(testIndPos, testIndAccel).fixedEstTrajLength = trajectoryLengthEstimated; + testInfo(testIndPos, testIndAccel).trajLengthFixedFrameGT = trajectoryLengthFixedFrameGT; + testInfo(testIndPos, testIndAccel).trajLengthRotFrameGT = trajectoryLengthRotatingFrameGT; close all; @@ -113,6 +132,10 @@ while testIndPos <= numPosTests posFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; rotFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); velFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + testInfo(testIndPos, testIndAccel).rotCoriolisPositionError = axisPositionsError(:,end); + testInfo(testIndPos, testIndAccel).rotCoriolisVelocityError = axisVelocityError(:,end); + testInfo(testIndPos, testIndAccel).rotCoriolisRotationError = rotationsErrorVectors(:,end); + testInfo(testIndPos, testIndAccel).rotCoriolisEstTrajLength = trajectoryLengthEstimated; close all; @@ -124,6 +147,10 @@ while testIndPos <= numPosTests posFinErrorRot(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; rotFinErrorRot(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); velFinErrorRot(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + testInfo(testIndPos, testIndAccel).rotPositionError = axisPositionsError(:,end); + testInfo(testIndPos, testIndAccel).rotVelocityError = axisVelocityError(:,end); + testInfo(testIndPos, testIndAccel).rotRotationError = rotationsErrorVectors(:,end); + testInfo(testIndPos, testIndAccel).rotEstTrajLength = trajectoryLengthEstimated; close all; catch From 284326649616ce4421bd8fb4496f7065de60cf41 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 25 Mar 2014 16:31:51 -0400 Subject: [PATCH 27/90] update to IMU factors (included flag to include 2nd order terms in integration) --- gtsam/navigation/CombinedImuFactor.h | 90 ++++++++++++------- gtsam/navigation/ImuFactor.h | 68 +++++++++----- .../tests/testCombinedImuFactor.cpp | 2 +- gtsam/navigation/tests/testImuFactor.cpp | 14 +-- 4 files changed, 114 insertions(+), 60 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 559568c84..345fbc5c9 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -66,8 +66,9 @@ namespace gtsam { Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + bool use2ndOrderIntegration_; ///< Controls the order of integration + ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] /** Default constructor, initialize with no IMU measurements */ @@ -78,12 +79,14 @@ namespace gtsam { const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements + const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements + const bool use2ndOrderIntegration = false ///< Controls the order of integration ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) ) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)), + use2ndOrderIntegration_(use2ndOrderIntegration) { // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), @@ -131,6 +134,19 @@ namespace gtsam { && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); } + void resetIntegration(){ + deltaPij = Vector3::Zero(); + deltaVij = Vector3::Zero(); + deltaRij = Rot3(); + deltaTij = 0.0; + delPdelBiasAcc = Matrix3::Zero(); + delPdelBiasOmega = Matrix3::Zero(); + delVdelBiasAcc = Matrix3::Zero(); + delVdelBiasOmega = Matrix3::Zero(); + delRdelBiasOmega = Matrix3::Zero(); + PreintMeasCov = Matrix::Zero(15,15); + } + /** Add a single IMU measurement to the preintegration. */ void integrateMeasurement( const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) @@ -158,11 +174,14 @@ namespace gtsam { // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - // delPdelBiasAcc += delVdelBiasAcc * deltaT; - // delPdelBiasOmega += delVdelBiasOmega * deltaT; - delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; - delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() - * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + if(!use2ndOrderIntegration_){ + delPdelBiasAcc += delVdelBiasAcc * deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT; + }else{ + delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() + * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + } delVdelBiasAcc += -deltaRij.matrix() * deltaT; delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; @@ -217,32 +236,31 @@ namespace gtsam { G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3); G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * - (H_vel_biasacc.transpose()); + (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(15,15,3,3) ) * + (H_vel_biasacc.transpose()); G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) * - (H_angles_biasomega.transpose()); + (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(18,18,3,3) ) * + (H_angles_biasomega.transpose()); G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3); G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3); - // OFF BLOCK DIAGONAL TERMS - Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3); - G_measCov_Gt.block(3,9,3,3) = block24; - G_measCov_Gt.block(9,3,3,3) = block24.transpose(); - - Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3); - G_measCov_Gt.block(6,12,3,3) = block35; - G_measCov_Gt.block(12,6,3,3) = block35.transpose(); + // NEW OFF BLOCK DIAGONAL TERMS + Matrix3 block23 = H_vel_biasacc * measurementCovariance.block(18,15,3,3) * H_angles_biasomega.transpose(); + G_measCov_Gt.block(3,6,3,3) = block23; + G_measCov_Gt.block(6,3,3,3) = block23.transpose(); PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt; // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - // deltaPij += deltaVij * deltaT; - deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + if(!use2ndOrderIntegration_){ + deltaPij += deltaVij * deltaT; + }else{ + deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + } deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; deltaTij += deltaT; @@ -309,7 +327,11 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ - typedef boost::shared_ptr shared_ptr; +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif /** Default constructor - only use for serialization */ CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} @@ -355,7 +377,7 @@ namespace gtsam { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) && equal_with_abs_tol(gravity_, e->gravity_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); @@ -365,6 +387,10 @@ namespace gtsam { const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } + const Vector3& gravity() const { return gravity_; } + + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + /** implement functions needed to derive from Factor */ /** vector of errors */ @@ -440,14 +466,13 @@ namespace gtsam { + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); - + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(), + //dBiasAcc/dVi + Matrix3::Zero(), + //dBiasOmega/dVi + Matrix3::Zero(); } if(H3) { @@ -519,7 +544,6 @@ namespace gtsam { Matrix3::Zero(), Matrix3::Identity(); } - // Evaluate residual error, according to [3] /* ---------------------------------------------------------------------------------------------------- */ const Vector3 fp = diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index b1e20297e..c69309e20 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -54,7 +54,7 @@ namespace gtsam { imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i) + Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) double deltaTij; ///< Time interval from i to j @@ -64,19 +64,20 @@ namespace gtsam { Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + bool use2ndOrderIntegration_; ///< Controls the order of integration /** Default constructor, initialize with no IMU measurements */ PreintegratedMeasurements( const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& integrationErrorCovariance ///< Covariance matrix of measuredAcc + const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc + const bool use2ndOrderIntegration = false ///< Controls the order of integration ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9) + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), use2ndOrderIntegration_(use2ndOrderIntegration) { measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), @@ -121,6 +122,19 @@ namespace gtsam { && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); } + void resetIntegration(){ + deltaPij = Vector3::Zero(); + deltaVij = Vector3::Zero(); + deltaRij = Rot3(); + deltaTij = 0.0; + delPdelBiasAcc = Matrix3::Zero(); + delPdelBiasOmega = Matrix3::Zero(); + delVdelBiasAcc = Matrix3::Zero(); + delVdelBiasOmega = Matrix3::Zero(); + delRdelBiasOmega = Matrix3::Zero(); + PreintMeasCov = Matrix::Zero(9,9); + } + /** Add a single IMU measurement to the preintegration. */ void integrateMeasurement( const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) @@ -137,11 +151,8 @@ namespace gtsam { // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame if(body_P_sensor){ Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); // linear acceleration vector in the body frame } @@ -153,16 +164,19 @@ namespace gtsam { // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - // delPdelBiasAcc += delVdelBiasAcc * deltaT; - // delPdelBiasOmega += delVdelBiasOmega * deltaT; - delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; - delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() - * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + if(!use2ndOrderIntegration_){ + delPdelBiasAcc += delVdelBiasAcc * deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT; + }else{ + delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() + * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + } delVdelBiasAcc += -deltaRij.matrix() * deltaT; delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; - // Update preintegrated mesurements covariance + // Update preintegrated measurements covariance /* ----------------------------------------------------------------------------------------------------------------------- */ Matrix3 Z_3x3 = Matrix3::Zero(); Matrix3 I_3x3 = Matrix3::Identity(); @@ -204,7 +218,11 @@ namespace gtsam { // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + if(!use2ndOrderIntegration_){ + deltaPij += deltaVij * deltaT; + }else{ + deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + } deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; deltaTij += deltaT; @@ -271,8 +289,11 @@ namespace gtsam { public: /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else typedef boost::shared_ptr shared_ptr; - +#endif /** Default constructor - only use for serialization */ ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} @@ -317,7 +338,7 @@ namespace gtsam { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) && equal_with_abs_tol(gravity_, e->gravity_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); @@ -327,6 +348,10 @@ namespace gtsam { const PreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } + const Vector3& gravity() const { return gravity_; } + + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + /** implement functions needed to derive from Factor */ /** vector of errors */ @@ -388,6 +413,7 @@ namespace gtsam { // dfR/dPi Matrix3::Zero(); } + if(H2) { H2->resize(9,3); (*H2) << @@ -396,11 +422,11 @@ namespace gtsam { + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(); - + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(); } + if(H3) { H3->resize(9,6); @@ -412,6 +438,7 @@ namespace gtsam { // dfR/dPosej Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); } + if(H4) { H4->resize(9,3); (*H4) << @@ -422,6 +449,7 @@ namespace gtsam { // dfR/dVj Matrix3::Zero(); } + if(H5) { const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 10d582287..a07539d1e 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -260,7 +260,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } #include diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 6cd286f68..a6894898b 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -136,8 +136,9 @@ TEST( ImuFactor, PreintegratedMeasurements ) Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0); double expectedDeltaT1(0.5); + bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij), 1e-6)); @@ -177,7 +178,8 @@ TEST( ImuFactor, Error ) Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + bool use2ndOrderIntegration = true; + ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor @@ -217,7 +219,7 @@ TEST( ImuFactor, Error ) Matrix H1atop6 = H1a.topRows(6); EXPECT(assert_equal(H1etop6, H1atop6)); // rotations - EXPECT(assert_equal(RH1e, H1a.bottomRows(3))); // evaluate only the rotation residue + EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H2e, H2a)); @@ -226,7 +228,7 @@ TEST( ImuFactor, Error ) Matrix H3atop6 = H3a.topRows(6); EXPECT(assert_equal(H3etop6, H3atop6)); // rotations - EXPECT(assert_equal(RH3e, H3a.bottomRows(3))); // evaluate only the rotation residue + EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H4e, H4a)); // EXPECT(assert_equal(H5e, H5a)); @@ -324,7 +326,7 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } @@ -436,7 +438,7 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } //#include From eaf298bd18ba4b05bbbc3228a36854ba3e881e0e Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 3 Apr 2014 11:34:26 -0400 Subject: [PATCH 28/90] starting covariance analysis on between factors (monte carlo runs) --- .../+imuSimulator/covarianceAnalysisBetween.m | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m new file mode 100644 index 000000000..667f26170 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -0,0 +1,44 @@ +% Test GTSAM covariances on a graph with betweenFactors + +clc +clear all +close all + +%% Create ground truth trajectory +trajectoryLength = 100; + +% possibly create random trajectory +currentPoseKey = symbol('x', 0); +currentPose = Pose3; +gtValues = Values; +gtValues.insert(currentPoseKey, currentPose); + +for i=1:trajectoryLength + currentPoseKey = symbol('x', i); + deltaPosition = % create random vector with mean [x 0 0] + deltaRotation = % create random rotation with mean [0 0 0] + deltaPose = Pose3(deltaRotation, Point3(deltaPosition)); + % "Deduce" ground truth measurements + % deltaPose are the gt measurements - save them in some structure + currentPose = currentPose.compose(deltaPose); + gtValues.insert(currentPoseKey, currentPose); +end + +%% Create gt graph (using between with ground truth measurements) +% Compute covariances using gtGraph and gtValues (for visualization) + +% decide measurement covariance + +%% for k=1:numMonteCarloRuns +% create a new graph +% for each measurement. add noise and add to graph +% optimize +% compute covariances: +% compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances +% "estimationError = estimatedValues - gtValues" only holds in a linear case +% in a nonlinear case estimationError = LogMap ((estimatedValues.inverse) * gtValues) +% in GTSAM you should check "localCoordinates" + +%% compute statistics: ANEES, plots + + From 93458eaeff3fa4263d6d0009ef34e2e477b7251d Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 3 Apr 2014 16:52:04 -0400 Subject: [PATCH 29/90] Added flag for second order coriolis terms (default is false) --- gtsam/navigation/CombinedImuFactor.h | 80 ++++++++++++++++++++------- gtsam/navigation/ImuFactor.h | 81 +++++++++++++++++----------- 2 files changed, 112 insertions(+), 49 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 345fbc5c9..bd0b9c03b 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -11,7 +11,7 @@ /** * @file CombinedImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman + * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen **/ #pragma once @@ -324,6 +324,8 @@ namespace gtsam { Vector3 omegaCoriolis_; boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + public: /** Shorthand for a smart pointer to a factor */ @@ -339,12 +341,13 @@ namespace gtsam { /** Constructor */ CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : + const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor) { + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis){ } virtual ~CombinedImuFactor() {} @@ -435,19 +438,18 @@ namespace gtsam { const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - if(H1) { - H1->resize(15,6); + /* (*H1) << // dfP/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), // dfP/dPi - - Rot_i.matrix(), + - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij, // dfV/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), // dfV/dPi - Matrix3::Zero(), + skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij, // dfR/dRi Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), // dfR/dPi @@ -456,6 +458,40 @@ namespace gtsam { Matrix3::Zero(), Matrix3::Zero(), //dBiasOmega/dPi Matrix3::Zero(), Matrix3::Zero(); + */ + if(H1) { + H1->resize(15,6); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Matrix3::Zero(); + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(), + //dBiasAcc/dPi + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPi + Matrix3::Zero(), Matrix3::Zero(); } if(H2) { @@ -578,7 +614,8 @@ namespace gtsam { static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false) { const double& deltaTij = preintegratedMeasurements.deltaTij; @@ -590,18 +627,23 @@ namespace gtsam { // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index c69309e20..32d099c3f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -11,7 +11,7 @@ /** * @file ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman + * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen **/ #pragma once @@ -286,6 +286,8 @@ namespace gtsam { Vector3 omegaCoriolis_; boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + public: /** Shorthand for a smart pointer to a factor */ @@ -301,12 +303,13 @@ namespace gtsam { /** Constructor */ ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none) : + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor) { + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis){ } virtual ~ImuFactor() {} @@ -397,21 +400,33 @@ namespace gtsam { if(H1) { H1->resize(9,6); - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - - Rot_i.matrix(), - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - Matrix3::Zero(), - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Matrix3::Zero(); + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(); } if(H2) { @@ -497,7 +512,8 @@ namespace gtsam { /** predicted states from IMU */ static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false) { const double& deltaTij = preintegratedMeasurements.deltaTij; @@ -509,18 +525,23 @@ namespace gtsam { // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) From d394ec5574beb045816efd36865945fb8da6d7f9 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Fri, 4 Apr 2014 17:00:20 -0400 Subject: [PATCH 30/90] Added ground truth factor graph creation. Added OdometryExample3D as a modified version of OdometryExample for reference (can be removed later) --- .../+imuSimulator/OdometryExample3D.m | 68 +++++++++++++++++++ .../+imuSimulator/covarianceAnalysisBetween.m | 40 +++++++++-- 2 files changed, 104 insertions(+), 4 deletions(-) create mode 100644 matlab/unstable_examples/+imuSimulator/OdometryExample3D.m diff --git a/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m b/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m new file mode 100644 index 000000000..d28d3c2cb --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m @@ -0,0 +1,68 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 Example of a simple 2D localization example +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Copied Original file. Modified by David Jensen to use Pose3 instead of +% Pose2. Everything else is the same. + +import gtsam.* + +%% Assumptions +% - Robot poses are facing along the X axis (horizontal, to the right in 2D) +% - The robot moves 2 meters each step +% - The robot is on a grid, moving 2 meters each step + +%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) +graph = NonlinearFactorGraph; + +%% Add a Gaussian prior on pose x_1 +priorMean = Pose3();%Pose3.Expmap([0.0; 0.0; 0.0; 0.0; 0.0; 0.0]); % prior mean is at origin +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.3; 0.1; 0.1; 0.1]); % 30cm std on x,y, 0.1 rad on theta +graph.add(PriorFactorPose3(1, priorMean, priorNoise)); % add directly to graph + +%% Add two odometry factors +odometry = Pose3.Expmap([0.0; 0.0; 0.0; 2.0; 0.0; 0.0]); % create a measurement for both factors (the same in this case) +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.2; 0.1; 0.1; 0.1]); % 20cm std on x,y, 0.1 rad on theta +graph.add(BetweenFactorPose3(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose3(2, 3, odometry, odometryNoise)); + +%% print +graph.print(sprintf('\nFactor graph:\n')); + +%% Initialize to noisy points +initialEstimate = Values; +%initialEstimate.insert(1, Pose3.Expmap([0.2; 0.1; 0.1; 0.5; 0.0; 0.0])); +%initialEstimate.insert(2, Pose3.Expmap([-0.2; 0.1; -0.1; 2.3; 0.1; 0.1])); +%initialEstimate.insert(3, Pose3.Expmap([0.1; -0.1; 0.1; 4.1; 0.1; -0.1])); +%initialEstimate.print(sprintf('\nInitial estimate:\n ')); + +for i=1:3 + deltaPosition = 0.5*rand(3,1) + [1;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 + deltaRotation = 0.1*rand(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) + deltaPose = Pose3.Expmap([deltaRotation; deltaPosition]); + currentPose = currentPose.compose(deltaPose); + initialEstimate.insert(i, currentPose); +end + +%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); +result.print(sprintf('\nFinal result:\n ')); + +%% Plot trajectory and covariance ellipses +cla; +hold on; + +plot3DTrajectory(result, [], Marginals(graph, result)); + +axis([-0.6 4.8 -1 1]) +axis equal +view(2) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 667f26170..f6c1744fd 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -1,3 +1,5 @@ +import gtsam.*; + % Test GTSAM covariances on a graph with betweenFactors clc @@ -5,26 +7,56 @@ clear all close all %% Create ground truth trajectory -trajectoryLength = 100; +trajectoryLength = 5; % possibly create random trajectory currentPoseKey = symbol('x', 0); currentPose = Pose3; gtValues = Values; gtValues.insert(currentPoseKey, currentPose); +gtGraph = NonlinearFactorGraph; +gtNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.1; 0.05; 0.05; 0.05]); % Noise for GT measurements for i=1:trajectoryLength currentPoseKey = symbol('x', i); - deltaPosition = % create random vector with mean [x 0 0] - deltaRotation = % create random rotation with mean [0 0 0] - deltaPose = Pose3(deltaRotation, Point3(deltaPosition)); + deltaPosition = 0.5*rand(3,1) + [1;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 + deltaRotation = 0.1*rand(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) + deltaPose = Pose3.Expmap([deltaRotation; deltaPosition]); + deltaPoseNoise = gtNoise; + % "Deduce" ground truth measurements % deltaPose are the gt measurements - save them in some structure + gtMeasurementPose(i) = deltaPose; currentPose = currentPose.compose(deltaPose); gtValues.insert(currentPoseKey, currentPose); + + % Add the factor to the factor graph + if(i == 1) + gtGraph.add(PriorFactorPose3(currentPoseKey, deltaPose, deltaPoseNoise)); + else + gtGraph.add(BetweenFactorPose3(previousPoseKey, currentPoseKey, deltaPose, deltaPoseNoise)); + end + previousPoseKey = currentPoseKey; end +gtGraph.print(sprintf('\nGround Truth - Factor graph:\n')); +gtValues.print(sprintf('\nGround Truth - Values:\n')); + %% Create gt graph (using between with ground truth measurements) +% Optimize using Levenberg-Marquardt +optimizer = LevenbergMarquardtOptimizer(gtGraph, gtValues); +gtResult = optimizer.optimizeSafely(); +gtResult.print(sprintf('\nGround Truth - Final Result:\n')); + +% Plot trajectory and covariance ellipses +% Couldn't get this to work in the modified example (OdometryExample3D). +% Something strange with 3D trajectories? +cla; +hold on; + +plot3DTrajectory(gtResult, [], Marginals(gtGraph, gtResult)); +axis equal + % Compute covariances using gtGraph and gtValues (for visualization) % decide measurement covariance From d325fd125135256392c9333ee564258f29e4717d Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 6 Apr 2014 21:05:13 -0400 Subject: [PATCH 31/90] working covariance example --- .../+imuSimulator/covarianceAnalysisBetween.m | 169 +++++++++++++----- 1 file changed, 126 insertions(+), 43 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index f6c1744fd..cdf3f1db3 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -1,6 +1,8 @@ import gtsam.*; % Test GTSAM covariances on a graph with betweenFactors +% Authors: Luca Carlone, David Jensen +% Date: 2014/4/6 clc clear all @@ -8,69 +10,150 @@ close all %% Create ground truth trajectory trajectoryLength = 5; +unsmooth_DP = 0.5; % controls smoothness on translation norm +unsmooth_DR = 0.1; % controls smoothness on translation norm % possibly create random trajectory -currentPoseKey = symbol('x', 0); -currentPose = Pose3; gtValues = Values; -gtValues.insert(currentPoseKey, currentPose); gtGraph = NonlinearFactorGraph; -gtNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.1; 0.05; 0.05; 0.05]); % Noise for GT measurements + +noiseVector = [0.01; 0.0001; 0.0001; 0.1; 0.1; 0.1]; +noise = noiseModel.Diagonal.Sigmas(noiseVector); + +currentPoseKey = symbol('x', 0); +currentPose = Pose3; % initial pose +gtValues.insert(currentPoseKey, currentPose); +gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); for i=1:trajectoryLength currentPoseKey = symbol('x', i); - deltaPosition = 0.5*rand(3,1) + [1;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 - deltaRotation = 0.1*rand(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) - deltaPose = Pose3.Expmap([deltaRotation; deltaPosition]); - deltaPoseNoise = gtNoise; + gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 + gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) + gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; + deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); % "Deduce" ground truth measurements % deltaPose are the gt measurements - save them in some structure - gtMeasurementPose(i) = deltaPose; currentPose = currentPose.compose(deltaPose); gtValues.insert(currentPoseKey, currentPose); - % Add the factor to the factor graph - if(i == 1) - gtGraph.add(PriorFactorPose3(currentPoseKey, deltaPose, deltaPoseNoise)); - else - gtGraph.add(BetweenFactorPose3(previousPoseKey, currentPoseKey, deltaPose, deltaPoseNoise)); - end - previousPoseKey = currentPoseKey; + % Add the factors to the factor graph + gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise)); end -gtGraph.print(sprintf('\nGround Truth - Factor graph:\n')); -gtValues.print(sprintf('\nGround Truth - Values:\n')); - -%% Create gt graph (using between with ground truth measurements) -% Optimize using Levenberg-Marquardt -optimizer = LevenbergMarquardtOptimizer(gtGraph, gtValues); -gtResult = optimizer.optimizeSafely(); -gtResult.print(sprintf('\nGround Truth - Final Result:\n')); - -% Plot trajectory and covariance ellipses -% Couldn't get this to work in the modified example (OdometryExample3D). -% Something strange with 3D trajectories? -cla; +figure(1) hold on; - -plot3DTrajectory(gtResult, [], Marginals(gtGraph, gtResult)); +plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); +% plot3DTrajectory(values,linespec,frames,scale,marginals) axis equal -% Compute covariances using gtGraph and gtValues (for visualization) +numMonteCarloRuns = 100; -% decide measurement covariance +for k=1:numMonteCarloRuns + % create a new graph + graph = NonlinearFactorGraph; + + % noisy prior + currentPoseKey = symbol('x', 0); + noisyDelta = noiseVector .* randn(6,1); + initialPose = Pose3.Expmap(noisyDelta); + graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise)); + + for i=1:trajectoryLength + currentPoseKey = symbol('x', i); + + % for each measurement. add noise and add to graph + noisyDelta = gtDeltaMatrix(i,:)' + (noiseVector .* randn(6,1)); + noisyDeltaPose = Pose3.Expmap(noisyDelta); + + % Add the factors to the factor graph + graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noise)); + end + + % optimize + optimizer = GaussNewtonOptimizer(graph, gtValues); + estimate = optimizer.optimize(); + + figure(1) + plot3DTrajectory(estimate, '-b'); + + marginals = Marginals(graph, estimate); + + % for each pose in the trajectory + for i=1:trajectoryLength+1 + % compute estimation errors + currentPoseKey = symbol('x', i-1); + gtPosition = gtValues.at(currentPoseKey).translation.vector; + estPosition = estimate.at(currentPoseKey).translation.vector; + errPosition = estPosition - gtPosition; + + % compute covariances: + cov = marginals.marginalCovariance(currentPoseKey); + covPosition = cov(4:6,4:6); + + % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances + NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof + end -%% for k=1:numMonteCarloRuns -% create a new graph -% for each measurement. add noise and add to graph -% optimize -% compute covariances: -% compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances -% "estimationError = estimatedValues - gtValues" only holds in a linear case -% in a nonlinear case estimationError = LogMap ((estimatedValues.inverse) * gtValues) -% in GTSAM you should check "localCoordinates" + figure(2) + hold on + plot(NEES(k,:),'-b','LineWidth',1.5) +end +ANEES = mean(NEES); +plot(ANEES,'-r','LineWidth',2) +plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof +box on +set(gca,'Fontsize',16) +title('NEES and ANEES'); -%% compute statistics: ANEES, plots +figure(1) +box on +title('Ground truth and estimates for each MC runs'); +set(gca,'Fontsize',16) +%% Let us compute statistics on the overall NEES +n = 3; % position vector dimension +N = numMonteCarloRuns; % number of runs +alpha = 0.01; % confidence level + +% mean_value = n*N; % mean value of the Chi-square distribution +% (we divide by n * N and for this reason we expect ANEES around 1) +r1 = chi2inv(alpha, n * N) / (n * N); +r2 = chi2inv(1-alpha, n * N) / (n * N); + +% output here +fprintf(1, 'r1 = %g\n', r1); +fprintf(1, 'r2 = %g\n', r2); + +figure(3) +hold on +plot(ANEES/n,'-b','LineWidth',2) +plot(ones(size(ANEES,2),1),'r-'); +plot(r1*ones(size(ANEES,2),1),'k-.'); +plot(r2*ones(size(ANEES,2),1),'k-.'); +box on +title('NEES normalized by dof VS bounds'); +set(gca,'Fontsize',16) + +%% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4) +% the nees for a single experiment (i) is defined as +% NEES_i = xtilda' * inv(P) * xtilda, +% where xtilda in R^n is the estimation +% error, and P is the covariance estimated by the approach we want to test +% +% Average NEES. Given N Monte Carlo simulations, i=1,...,N, the average +% NEES is: +% ANEES = sum(NEES_i)/N +% The quantity N*ANEES is distributed according to a Chi-square +% distribution with N*n degrees of freedom. +% +% For the single run case, N=1, therefore NEES = ANEES is distributed +% according to a chi-square distribution with n degrees of freedom (e.g. n=3 +% if we are testing a position estimate) +% Therefore its mean should be n (difficult to see from a single run) +% and, with probability alpha, it should hold: +% +% NEES in [r1, r2] +% +% where r1 and r2 are built from the Chi-square distribution From dfda7fad89aef6dd27a3a82e372d67ee0dc58e1a Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 6 Apr 2014 21:43:53 -0400 Subject: [PATCH 32/90] minor changes to matlab covariance test --- .../+imuSimulator/covarianceAnalysisBetween.m | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index cdf3f1db3..1701de64b 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -9,15 +9,17 @@ clear all close all %% Create ground truth trajectory -trajectoryLength = 5; +trajectoryLength = 49; unsmooth_DP = 0.5; % controls smoothness on translation norm unsmooth_DR = 0.1; % controls smoothness on translation norm -% possibly create random trajectory +% possibly create random trajectory as ground Truth gtValues = Values; gtGraph = NonlinearFactorGraph; -noiseVector = [0.01; 0.0001; 0.0001; 0.1; 0.1; 0.1]; +sigma_ang = 1e-2; +sigma_cart = 0.1; +noiseVector = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noise = noiseModel.Diagonal.Sigmas(noiseVector); currentPoseKey = symbol('x', 0); @@ -44,11 +46,9 @@ end figure(1) hold on; plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); -% plot3DTrajectory(values,linespec,frames,scale,marginals) axis equal numMonteCarloRuns = 100; - for k=1:numMonteCarloRuns % create a new graph graph = NonlinearFactorGraph; @@ -62,7 +62,7 @@ for k=1:numMonteCarloRuns for i=1:trajectoryLength currentPoseKey = symbol('x', i); - % for each measurement. add noise and add to graph + % for each measurement: add noise and add to graph noisyDelta = gtDeltaMatrix(i,:)' + (noiseVector .* randn(6,1)); noisyDeltaPose = Pose3.Expmap(noisyDelta); @@ -99,6 +99,7 @@ for k=1:numMonteCarloRuns hold on plot(NEES(k,:),'-b','LineWidth',1.5) end +%% ANEES = mean(NEES); plot(ANEES,'-r','LineWidth',2) plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof @@ -106,10 +107,11 @@ box on set(gca,'Fontsize',16) title('NEES and ANEES'); +%% figure(1) box on -title('Ground truth and estimates for each MC runs'); set(gca,'Fontsize',16) +title('Ground truth and estimates for each MC runs'); %% Let us compute statistics on the overall NEES n = 3; % position vector dimension @@ -132,8 +134,8 @@ plot(ones(size(ANEES,2),1),'r-'); plot(r1*ones(size(ANEES,2),1),'k-.'); plot(r2*ones(size(ANEES,2),1),'k-.'); box on -title('NEES normalized by dof VS bounds'); set(gca,'Fontsize',16) +title('NEES normalized by dof VS bounds'); %% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4) % the nees for a single experiment (i) is defined as From ed6788fff43f8457f1a029ec9be3b6d74c0dd630 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 7 Apr 2014 11:56:22 -0400 Subject: [PATCH 33/90] fixed bug --- .../+imuSimulator/covarianceAnalysisBetween.m | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 1701de64b..bbd5dfff3 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -85,11 +85,12 @@ for k=1:numMonteCarloRuns currentPoseKey = symbol('x', i-1); gtPosition = gtValues.at(currentPoseKey).translation.vector; estPosition = estimate.at(currentPoseKey).translation.vector; + estR = estimate.at(currentPoseKey).rotation.matrix; errPosition = estPosition - gtPosition; % compute covariances: cov = marginals.marginalCovariance(currentPoseKey); - covPosition = cov(4:6,4:6); + covPosition = estR * cov(4:6,4:6) * estR'; % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof From d677d1781d054249d0a97790eca6a2dac1d1b99d Mon Sep 17 00:00:00 2001 From: djensen3 Date: Mon, 7 Apr 2014 23:12:31 -0400 Subject: [PATCH 34/90] Added flag to use scenario 2 ground truth data --- .../+imuSimulator/LatLonHRad_to_ECEF.m | 26 ++++ .../+imuSimulator/covarianceAnalysisBetween.m | 116 +++++++++++++----- 2 files changed, 112 insertions(+), 30 deletions(-) create mode 100644 matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m diff --git a/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m b/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m new file mode 100644 index 000000000..bcc163eba --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m @@ -0,0 +1,26 @@ +function pos_ECEF = LatLonHRad_to_ECEF(LatLonH) +% convert latitude, longitude, height to XYZ in ECEF coordinates +% LatLonH(1) : latitude in radian +% LatLonH(2) : longitude in radian +% LatLonH(3) : height in meter +% +% Source: A. Chatfield, "Fundamentals of High Accuracy Inertial +% Navigation", 1997. pp. 10 Eq 1.18 +% + +% constants +a = 6378137.0; %m +e_sqr =0.006694379990141317; +% b = 6356752.3142; % m + +%RAD_PER_DEG = pi/180; +phi = LatLonH(1);%*RAD_PER_DEG; +lambda = LatLonH(2);%*RAD_PER_DEG; +h = LatLonH(3); + +RN = a/sqrt(1 - e_sqr*sin(phi)^2); + +pos_ECEF = zeros(3,1); +pos_ECEF(1) = (RN + h )*cos(phi)*cos(lambda); +pos_ECEF(2) = (RN + h )*cos(phi)*sin(lambda); +pos_ECEF(3) = (RN*(1-e_sqr) + h)*sin(phi) ; \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index bbd5dfff3..cb2db2965 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -8,62 +8,118 @@ clc clear all close all +%% Configuration +useAspnData = 1; % controls whether or not to use the ASPN data for scenario 2 as the ground truth traj + %% Create ground truth trajectory trajectoryLength = 49; unsmooth_DP = 0.5; % controls smoothness on translation norm -unsmooth_DR = 0.1; % controls smoothness on translation norm +unsmooth_DR = 0.1; % controls smoothness on rotation norm -% possibly create random trajectory as ground Truth gtValues = Values; gtGraph = NonlinearFactorGraph; -sigma_ang = 1e-2; -sigma_cart = 0.1; +if useAspnData == 1 + sigma_ang = 1e-4; + sigma_cart = 40; +else + sigma_ang = 1e-2; + sigma_cart = 0.1; +end noiseVector = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noise = noiseModel.Diagonal.Sigmas(noiseVector); -currentPoseKey = symbol('x', 0); -currentPose = Pose3; % initial pose -gtValues.insert(currentPoseKey, currentPose); -gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); - -for i=1:trajectoryLength - currentPoseKey = symbol('x', i); - gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 - gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) - gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; - deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); - - % "Deduce" ground truth measurements - % deltaPose are the gt measurements - save them in some structure - currentPose = currentPose.compose(deltaPose); +if useAspnData == 1 +% Create a ground truth trajectory using scenario 2 data + fprintf('\nUsing Scenario 2 ground truth data\n'); + % load scenario 2 ground truth data + gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading'); + + % Add first pose + currentPoseKey = symbol('x', 0); + initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); + initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; + currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose gtValues.insert(currentPoseKey, currentPose); + gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); + prevPose = currentPose; - % Add the factors to the factor graph - gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise)); -end + % Limit the trajectory length + trajectoryLength = min([length(gtScenario2.Lat) trajectoryLength]); + + for i=2:trajectoryLength + currentPoseKey = symbol('x', i-1); + gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(i); gtScenario2.Lon(i); gtScenario2.Alt(i)]); + gtRotation = [gtScenario2.Roll(i); gtScenario2.Pitch(i); gtScenario2.Heading(i)]; + currentPose = Pose3.Expmap([gtRotation; gtECEF]); + + % Generate measurements as the current pose measured in the frame of + % the previous pose + deltaPose = prevPose.between(currentPose); + gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); + prevPose = currentPose; + + % Add values + gtValues.insert(currentPoseKey, currentPose); + + % Add the factor to the factor graph + gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise)); + end +else +% Create a random trajectory as ground truth + fprintf('\nCreating a random ground truth trajectory\n'); + % Add first pose + currentPoseKey = symbol('x', 0); + currentPose = Pose3; % initial pose + gtValues.insert(currentPoseKey, currentPose); + gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); + for i=1:trajectoryLength + currentPoseKey = symbol('x', i); + gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 + gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) + gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; + deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); + + % "Deduce" ground truth measurements + % deltaPose are the gt measurements - save them in some structure + currentPose = currentPose.compose(deltaPose); + gtValues.insert(currentPoseKey, currentPose); + + % Add the factors to the factor graph + gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise)); + end +end figure(1) hold on; plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); axis equal -numMonteCarloRuns = 100; +numMonteCarloRuns = 10; for k=1:numMonteCarloRuns + fprintf('Monte Carlo Run %d.\n', k'); % create a new graph graph = NonlinearFactorGraph; % noisy prior - currentPoseKey = symbol('x', 0); - noisyDelta = noiseVector .* randn(6,1); - initialPose = Pose3.Expmap(noisyDelta); - graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise)); + if useAspnData == 1 + currentPoseKey = symbol('x', 0); + initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); + initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; + initialPose = Pose3.Expmap([initialRotation; initialPosition] + (noiseVector .* randn(6,1))); % initial noisy pose + graph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); + else + currentPoseKey = symbol('x', 0); + noisyDelta = noiseVector .* randn(6,1); + initialPose = Pose3.Expmap(noisyDelta); + graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise)); + end - for i=1:trajectoryLength + for i=1:size(gtDeltaMatrix,1) currentPoseKey = symbol('x', i); % for each measurement: add noise and add to graph - noisyDelta = gtDeltaMatrix(i,:)' + (noiseVector .* randn(6,1)); + noisyDelta = gtDeltaMatrix(i,:)';% + (noiseVector .* randn(6,1)); noisyDeltaPose = Pose3.Expmap(noisyDelta); % Add the factors to the factor graph @@ -80,7 +136,7 @@ for k=1:numMonteCarloRuns marginals = Marginals(graph, estimate); % for each pose in the trajectory - for i=1:trajectoryLength+1 + for i=1:size(gtDeltaMatrix,1)+1 % compute estimation errors currentPoseKey = symbol('x', i-1); gtPosition = gtValues.at(currentPoseKey).translation.vector; From a58d00c0f951d25acb88e0cf59602ec7f14fd6a4 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 9 Apr 2014 11:34:59 -0400 Subject: [PATCH 35/90] Fixed bug --- .../+imuSimulator/covarianceAnalysisBetween.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index cb2db2965..b8157593e 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -12,7 +12,7 @@ close all useAspnData = 1; % controls whether or not to use the ASPN data for scenario 2 as the ground truth traj %% Create ground truth trajectory -trajectoryLength = 49; +trajectoryLength = 100; unsmooth_DP = 0.5; % controls smoothness on translation norm unsmooth_DR = 0.1; % controls smoothness on rotation norm @@ -107,7 +107,7 @@ for k=1:numMonteCarloRuns initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; initialPose = Pose3.Expmap([initialRotation; initialPosition] + (noiseVector .* randn(6,1))); % initial noisy pose - graph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); + graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise)); else currentPoseKey = symbol('x', 0); noisyDelta = noiseVector .* randn(6,1); @@ -119,7 +119,7 @@ for k=1:numMonteCarloRuns currentPoseKey = symbol('x', i); % for each measurement: add noise and add to graph - noisyDelta = gtDeltaMatrix(i,:)';% + (noiseVector .* randn(6,1)); + noisyDelta = gtDeltaMatrix(i,:)' + (noiseVector .* randn(6,1)); noisyDeltaPose = Pose3.Expmap(noisyDelta); % Add the factors to the factor graph From a92b3b2339b206001df451295f3ff00ac1f116a0 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 10 Apr 2014 13:26:10 -0400 Subject: [PATCH 36/90] Added IMU factors to ground truth factor graph. --- .../+imuSimulator/covarianceAnalysisBetween.m | 91 +++++++++++++++---- 1 file changed, 74 insertions(+), 17 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index b8157593e..5c6c2b84f 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -9,10 +9,30 @@ clear all close all %% Configuration -useAspnData = 1; % controls whether or not to use the ASPN data for scenario 2 as the ground truth traj +useAspnData = 0; % controls whether or not to use the ASPN data for scenario 2 as the ground truth traj +includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the random trajectory +includeCameraFactors = 0; +trajectoryLength = 50; + +deltaT = 1.0; % amount of time between IMU measurements +vel = [0 0 0]; % initial velocity (used for generating IMU measurements +g = [0; 0; 0]; % gravity +omegaCoriolis = [0; 0; 0]; % Coriolis + +% Imu metadata +epsBias = 1e-20; +zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of interest and is set to zero +IMU_metadata.AccelerometerSigma = 1e-5; +IMU_metadata.GyroscopeSigma = 1e-7; +IMU_metadata.IntegrationSigma = 1e-10; +IMU_metadata.BiasAccelerometerSigma = epsBias; +IMU_metadata.BiasGyroscopeSigma = epsBias; +IMU_metadata.BiasAccOmegaInit = epsBias; + +noiseVel = noiseModel.Isotropic.Sigma(3, 1e-10); +noiseBias = noiseModel.Isotropic.Sigma(6, epsBias); %% Create ground truth trajectory -trajectoryLength = 100; unsmooth_DP = 0.5; % controls smoothness on translation norm unsmooth_DR = 0.1; % controls smoothness on rotation norm @@ -26,11 +46,11 @@ else sigma_ang = 1e-2; sigma_cart = 0.1; end -noiseVector = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; -noise = noiseModel.Diagonal.Sigmas(noiseVector); +noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; +noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); if useAspnData == 1 -% Create a ground truth trajectory using scenario 2 data +%% Create a ground truth trajectory using scenario 2 data fprintf('\nUsing Scenario 2 ground truth data\n'); % load scenario 2 ground truth data gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading'); @@ -41,7 +61,7 @@ if useAspnData == 1 initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose gtValues.insert(currentPoseKey, currentPose); - gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); + gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); prevPose = currentPose; % Limit the trajectory length @@ -63,19 +83,31 @@ if useAspnData == 1 gtValues.insert(currentPoseKey, currentPose); % Add the factor to the factor graph - gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise)); + gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); end else -% Create a random trajectory as ground truth +%% Create a random trajectory as ground truth fprintf('\nCreating a random ground truth trajectory\n'); - % Add first pose + % Add priors currentPoseKey = symbol('x', 0); currentPose = Pose3; % initial pose gtValues.insert(currentPoseKey, currentPose); - gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noise)); + gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); + if includeIMUFactors == 1 + currentVelKey = symbol('v', 0); + currentBiasKey = symbol('b', 0); + gtValues.insert(currentVelKey, LieVector(vel')); + gtValues.insert(currentBiasKey, zeroBias); + gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(vel'), noiseVel)); + gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, noiseBias)); + end + for i=1:trajectoryLength currentPoseKey = symbol('x', i); + currentVelKey = symbol('v', i); + currentBiasKey = symbol('b', i); + gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; @@ -87,15 +119,40 @@ else gtValues.insert(currentPoseKey, currentPose); % Add the factors to the factor graph - gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noise)); + gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); + + % Add IMU factors + if includeIMUFactors == 1 + % create accel and gyro measurements based on + gyro = gtDeltaMatrix(i, 1:3)./deltaT; + accel = (gtDeltaMatrix(i, 4:6) - vel.*deltaT).*(2/(deltaT*deltaT)); + vel = gtDeltaMatrix(i,4:6)./deltaT; + imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... + zeroBias, ... + IMU_metadata.AccelerometerSigma.^2 * eye(3), ... + IMU_metadata.GyroscopeSigma.^2 * eye(3), ... + IMU_metadata.IntegrationSigma.^2 * eye(3)); + imuMeasurement.integrateMeasurement(accel', gyro', deltaT); + gtGraph.add(ImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, imuMeasurement, g, omegaCoriolis)); + gtGraph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ... + noiseModel.Isotropic.Sigma(6, epsBias))); + gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ... + noiseModel.Isotropic.Sigma(6, epsBias))); + gtValues.insert(currentVelKey, LieVector(vel')); + gtValues.insert(currentBiasKey, zeroBias); + end end + end figure(1) hold on; plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); axis equal -numMonteCarloRuns = 10; +numMonteCarloRuns = 100; for k=1:numMonteCarloRuns fprintf('Monte Carlo Run %d.\n', k'); % create a new graph @@ -107,23 +164,23 @@ for k=1:numMonteCarloRuns initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; initialPose = Pose3.Expmap([initialRotation; initialPosition] + (noiseVector .* randn(6,1))); % initial noisy pose - graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise)); + graph.add(PriorFactorPose3(currentPoseKey, initialPose, noisePose)); else currentPoseKey = symbol('x', 0); - noisyDelta = noiseVector .* randn(6,1); + noisyDelta = noiseVectorPose .* randn(6,1); initialPose = Pose3.Expmap(noisyDelta); - graph.add(PriorFactorPose3(currentPoseKey, initialPose, noise)); + graph.add(PriorFactorPose3(currentPoseKey, initialPose, noisePose)); end for i=1:size(gtDeltaMatrix,1) currentPoseKey = symbol('x', i); % for each measurement: add noise and add to graph - noisyDelta = gtDeltaMatrix(i,:)' + (noiseVector .* randn(6,1)); + noisyDelta = gtDeltaMatrix(i,:)' + (noiseVectorPose .* randn(6,1)); noisyDeltaPose = Pose3.Expmap(noisyDelta); % Add the factors to the factor graph - graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noise)); + graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose)); end % optimize From c20dd18ab718e7a4f18bc54826d10e4fe3aee318 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 10 Apr 2014 18:48:29 -0400 Subject: [PATCH 37/90] renaming --- .../+imuSimulator/covarianceAnalysisBetween.m | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 5c6c2b84f..cf996c038 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -9,7 +9,7 @@ clear all close all %% Configuration -useAspnData = 0; % controls whether or not to use the ASPN data for scenario 2 as the ground truth traj +useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the random trajectory includeCameraFactors = 0; trajectoryLength = 50; @@ -39,7 +39,7 @@ unsmooth_DR = 0.1; % controls smoothness on rotation norm gtValues = Values; gtGraph = NonlinearFactorGraph; -if useAspnData == 1 +if useRealData == 1 sigma_ang = 1e-4; sigma_cart = 40; else @@ -49,7 +49,7 @@ end noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); -if useAspnData == 1 +if useRealData == 1 %% Create a ground truth trajectory using scenario 2 data fprintf('\nUsing Scenario 2 ground truth data\n'); % load scenario 2 ground truth data @@ -159,7 +159,7 @@ for k=1:numMonteCarloRuns graph = NonlinearFactorGraph; % noisy prior - if useAspnData == 1 + if useRealData == 1 currentPoseKey = symbol('x', 0); initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; From 24157ca1245c084ce518f6367ad38ebee42098f7 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 10 Apr 2014 19:26:53 -0400 Subject: [PATCH 38/90] working on imu test --- .../+imuSimulator/covarianceAnalysisBetween.m | 192 ++++++++++-------- 1 file changed, 105 insertions(+), 87 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index cf996c038..a159ed13e 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -9,91 +9,98 @@ clear all close all %% Configuration -useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj -includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the random trajectory -includeCameraFactors = 0; -trajectoryLength = 50; +useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj +includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the random trajectory +% includeCameraFactors = 0; % not implemented yet +trajectoryLength = 2; % length of the ground truth trajectory -deltaT = 1.0; % amount of time between IMU measurements -vel = [0 0 0]; % initial velocity (used for generating IMU measurements -g = [0; 0; 0]; % gravity -omegaCoriolis = [0; 0; 0]; % Coriolis - -% Imu metadata +%% Imu metadata epsBias = 1e-20; -zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of interest and is set to zero +zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.GyroscopeSigma = 1e-7; IMU_metadata.IntegrationSigma = 1e-10; IMU_metadata.BiasAccelerometerSigma = epsBias; IMU_metadata.BiasGyroscopeSigma = epsBias; IMU_metadata.BiasAccOmegaInit = epsBias; - -noiseVel = noiseModel.Isotropic.Sigma(3, 1e-10); +noiseVel = noiseModel.Isotropic.Sigma(3, 0.1); noiseBias = noiseModel.Isotropic.Sigma(6, epsBias); -%% Create ground truth trajectory -unsmooth_DP = 0.5; % controls smoothness on translation norm -unsmooth_DR = 0.1; % controls smoothness on rotation norm - -gtValues = Values; -gtGraph = NonlinearFactorGraph; - +%% Between metadata if useRealData == 1 - sigma_ang = 1e-4; - sigma_cart = 40; + sigma_ang = 1e-4; sigma_cart = 40; else - sigma_ang = 1e-2; - sigma_cart = 0.1; + sigma_ang = 1e-2; sigma_cart = 0.1; end noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); -if useRealData == 1 -%% Create a ground truth trajectory using scenario 2 data - fprintf('\nUsing Scenario 2 ground truth data\n'); - % load scenario 2 ground truth data - gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading'); - - % Add first pose - currentPoseKey = symbol('x', 0); - initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); - initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; - currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose - gtValues.insert(currentPoseKey, currentPose); - gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); - prevPose = currentPose; - - % Limit the trajectory length - trajectoryLength = min([length(gtScenario2.Lat) trajectoryLength]); - - for i=2:trajectoryLength - currentPoseKey = symbol('x', i-1); - gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(i); gtScenario2.Lon(i); gtScenario2.Alt(i)]); - gtRotation = [gtScenario2.Roll(i); gtScenario2.Pitch(i); gtScenario2.Heading(i)]; - currentPose = Pose3.Expmap([gtRotation; gtECEF]); - - % Generate measurements as the current pose measured in the frame of - % the previous pose - deltaPose = prevPose.between(currentPose); - gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); - prevPose = currentPose; - - % Add values - gtValues.insert(currentPoseKey, currentPose); - - % Add the factor to the factor graph - gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); - end -else -%% Create a random trajectory as ground truth - fprintf('\nCreating a random ground truth trajectory\n'); - % Add priors - currentPoseKey = symbol('x', 0); - currentPose = Pose3; % initial pose - gtValues.insert(currentPoseKey, currentPose); - gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); +%% Create ground truth trajectory +gtValues = Values; +gtGraph = NonlinearFactorGraph; +if useRealData == 1 + % % % %% Create a ground truth trajectory from Real data (if available) + % % % fprintf('\nUsing real data as ground truth\n'); + % % % gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading'); + % Time: [4201x1 double] + % Lat: [4201x1 double] + % Lon: [4201x1 double] + % Alt: [4201x1 double] + % VEast: [4201x1 double] + % VNorth: [4201x1 double] + % VUp: [4201x1 double] + % Roll: [4201x1 double] + % Pitch: [4201x1 double] + % Heading + % % % + % % % % Add first pose + % % % currentPoseKey = symbol('x', 0); + % % % initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); + % % % initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; + % % % currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose + % % % gtValues.insert(currentPoseKey, currentPose); + % % % gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); + % % % prevPose = currentPose; + % % % + % % % % Limit the trajectory length + % % % trajectoryLength = min([length(gtScenario2.Lat) trajectoryLength]); + % % % + % % % for i=2:trajectoryLength + % % % currentPoseKey = symbol('x', i-1); + % % % gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(i); gtScenario2.Lon(i); gtScenario2.Alt(i)]); + % % % gtRotation = [gtScenario2.Roll(i); gtScenario2.Pitch(i); gtScenario2.Heading(i)]; + % % % currentPose = Pose3.Expmap([gtRotation; gtECEF]); + % % % + % % % % Generate measurements as the current pose measured in the frame of + % % % % the previous pose + % % % deltaPose = prevPose.between(currentPose); + % % % gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); + % % % prevPose = currentPose; + % % % + % % % % Add values + % % % gtValues.insert(currentPoseKey, currentPose); + % % % + % % % % Add the factor to the factor graph + % % % gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); + % % % end +else + %% Create a random trajectory as ground truth + currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements) + currentPose = Pose3; % initial pose % initial pose + deltaT = 1.0; % amount of time between IMU measurements + g = [0; 0; 0]; % gravity + omegaCoriolis = [0; 0; 0]; % Coriolis + + unsmooth_DP = 0.5; % controls smoothness on translation norm + unsmooth_DR = 0.1; % controls smoothness on rotation norm + + fprintf('\nCreating a random ground truth trajectory\n'); + %% Add priors + currentPoseKey = symbol('x', 0); + gtValues.insert(currentPoseKey, currentPose); + gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); + if includeIMUFactors == 1 currentVelKey = symbol('v', 0); currentBiasKey = symbol('b', 0); @@ -105,28 +112,31 @@ else for i=1:trajectoryLength currentPoseKey = symbol('x', i); - currentVelKey = symbol('v', i); - currentBiasKey = symbol('b', i); gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; - deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); - + measurements.deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); + % "Deduce" ground truth measurements % deltaPose are the gt measurements - save them in some structure currentPose = currentPose.compose(deltaPose); gtValues.insert(currentPoseKey, currentPose); - - % Add the factors to the factor graph + + % Add the factors to the factor graph gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); % Add IMU factors if includeIMUFactors == 1 + currentVelKey = symbol('v', i); % not used if includeIMUFactors is false + currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false + % create accel and gyro measurements based on - gyro = gtDeltaMatrix(i, 1:3)./deltaT; - accel = (gtDeltaMatrix(i, 4:6) - vel.*deltaT).*(2/(deltaT*deltaT)); - vel = gtDeltaMatrix(i,4:6)./deltaT; + measurements.imu.gyro = gtDeltaMatrix(i, 1:3)./deltaT; + % acc = (deltaPosition - initialVel * dT) * (2/dt^2) + measurements.imu.accel = (gtDeltaMatrix(i, 4:6) - currentVel.*deltaT).*(2/(deltaT*deltaT)); + % update current velocity + currentVel = gtDeltaMatrix(i,4:6)./deltaT; imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... zeroBias, ... IMU_metadata.AccelerometerSigma.^2 * eye(3), ... @@ -147,9 +157,17 @@ else end end + +gtPoses = Values; +for i=0:trajectoryLength + currentPoseKey = symbol('x', i); + currentPose = gtValues.at(currentPoseKey); + gtPoses.insert(currentPoseKey, currentPose); +end + figure(1) hold on; -plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); +plot3DTrajectory(gtPoses, '-r', [], 1, Marginals(gtGraph, gtPoses)); axis equal numMonteCarloRuns = 100; @@ -186,7 +204,7 @@ for k=1:numMonteCarloRuns % optimize optimizer = GaussNewtonOptimizer(graph, gtValues); estimate = optimizer.optimize(); - + figure(1) plot3DTrajectory(estimate, '-b'); @@ -208,7 +226,7 @@ for k=1:numMonteCarloRuns % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof end - + figure(2) hold on plot(NEES(k,:),'-b','LineWidth',1.5) @@ -216,7 +234,7 @@ end %% ANEES = mean(NEES); plot(ANEES,'-r','LineWidth',2) -plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof +plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof box on set(gca,'Fontsize',16) title('NEES and ANEES'); @@ -232,7 +250,7 @@ n = 3; % position vector dimension N = numMonteCarloRuns; % number of runs alpha = 0.01; % confidence level -% mean_value = n*N; % mean value of the Chi-square distribution +% mean_value = n*N; % mean value of the Chi-square distribution % (we divide by n * N and for this reason we expect ANEES around 1) r1 = chi2inv(alpha, n * N) / (n * N); r2 = chi2inv(1-alpha, n * N) / (n * N); @@ -252,19 +270,19 @@ set(gca,'Fontsize',16) title('NEES normalized by dof VS bounds'); %% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4) -% the nees for a single experiment (i) is defined as -% NEES_i = xtilda' * inv(P) * xtilda, +% the nees for a single experiment (i) is defined as +% NEES_i = xtilda' * inv(P) * xtilda, % where xtilda in R^n is the estimation % error, and P is the covariance estimated by the approach we want to test -% +% % Average NEES. Given N Monte Carlo simulations, i=1,...,N, the average % NEES is: % ANEES = sum(NEES_i)/N % The quantity N*ANEES is distributed according to a Chi-square % distribution with N*n degrees of freedom. % -% For the single run case, N=1, therefore NEES = ANEES is distributed -% according to a chi-square distribution with n degrees of freedom (e.g. n=3 +% For the single run case, N=1, therefore NEES = ANEES is distributed +% according to a chi-square distribution with n degrees of freedom (e.g. n=3 % if we are testing a position estimate) % Therefore its mean should be n (difficult to see from a single run) % and, with probability alpha, it should hold: From 46c6d41cd6437364163330ef87d61eb01bb95793 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 10 Apr 2014 22:00:07 -0400 Subject: [PATCH 39/90] fixed test on real data (gt) --- .../+imuSimulator/covarianceAnalysisBetween.m | 172 ++++++++++-------- .../unstable_examples/+imuSimulator/ct2ENU.m | 55 ++++++ 2 files changed, 148 insertions(+), 79 deletions(-) create mode 100644 matlab/unstable_examples/+imuSimulator/ct2ENU.m diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index a159ed13e..bcf4fc124 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -9,13 +9,13 @@ clear all close all %% Configuration -useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj -includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the random trajectory +useRealData = 1; % controls whether or not to use the Real data (is available) as the ground truth traj +includeIMUFactors = 0; % if true, IMU type 1 Factors will be generated for the random trajectory % includeCameraFactors = 0; % not implemented yet -trajectoryLength = 2; % length of the ground truth trajectory +trajectoryLength = 210; % length of the ground truth trajectory %% Imu metadata -epsBias = 1e-20; +epsBias = 1e-7; zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.GyroscopeSigma = 1e-7; @@ -28,7 +28,7 @@ noiseBias = noiseModel.Isotropic.Sigma(6, epsBias); %% Between metadata if useRealData == 1 - sigma_ang = 1e-4; sigma_cart = 40; + sigma_ang = 1e-4; sigma_cart = 0.01; else sigma_ang = 1e-2; sigma_cart = 0.1; end @@ -40,55 +40,58 @@ gtValues = Values; gtGraph = NonlinearFactorGraph; if useRealData == 1 - % % % %% Create a ground truth trajectory from Real data (if available) - % % % fprintf('\nUsing real data as ground truth\n'); - % % % gtScenario2 = load('truth_scen2.mat', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading'); - % Time: [4201x1 double] - % Lat: [4201x1 double] - % Lon: [4201x1 double] - % Alt: [4201x1 double] - % VEast: [4201x1 double] - % VNorth: [4201x1 double] - % VUp: [4201x1 double] - % Roll: [4201x1 double] - % Pitch: [4201x1 double] - % Heading - % % % - % % % % Add first pose - % % % currentPoseKey = symbol('x', 0); - % % % initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); - % % % initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; - % % % currentPose = Pose3.Expmap([initialRotation; initialPosition]); % initial pose - % % % gtValues.insert(currentPoseKey, currentPose); - % % % gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); - % % % prevPose = currentPose; - % % % - % % % % Limit the trajectory length - % % % trajectoryLength = min([length(gtScenario2.Lat) trajectoryLength]); - % % % - % % % for i=2:trajectoryLength - % % % currentPoseKey = symbol('x', i-1); - % % % gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(i); gtScenario2.Lon(i); gtScenario2.Alt(i)]); - % % % gtRotation = [gtScenario2.Roll(i); gtScenario2.Pitch(i); gtScenario2.Heading(i)]; - % % % currentPose = Pose3.Expmap([gtRotation; gtECEF]); - % % % - % % % % Generate measurements as the current pose measured in the frame of - % % % % the previous pose - % % % deltaPose = prevPose.between(currentPose); - % % % gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); - % % % prevPose = currentPose; - % % % - % % % % Add values - % % % gtValues.insert(currentPoseKey, currentPose); - % % % - % % % % Add the factor to the factor graph - % % % gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); - % % % end + subsampleStep = 20; + + %% Create a ground truth trajectory from Real data (if available) + fprintf('\nUsing real data as ground truth\n'); + gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',... + 'VEast', 'VNorth', 'VUp'); + + Org_lat = gtScenario.Lat(1); + Org_lon = gtScenario.Lon(1); + initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]); + + % Limit the trajectory length + trajectoryLength = min([length(gtScenario.Lat) trajectoryLength]); + + for i=1:trajectoryLength + currentPoseKey = symbol('x', i-1); + scenarioInd = subsampleStep * (i-1) + 1 + gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); + % truth in ENU + dX = gtECEF(1) - initialPositionECEF(1); + dY = gtECEF(2) - initialPositionECEF(2); + dZ = gtECEF(3) - initialPositionECEF(3); + [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); + + gtPosition = [xlt, ylt, zlt]'; + gtRotation = Rot3; % Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); + currentPose = Pose3(gtRotation, Point3(gtPosition)); + + % Add values + gtValues.insert(currentPoseKey, currentPose); + + if i==1 % first time step, add priors + warning('roll-pitch-yaw is different from Rodriguez') + warning('using identity rotation') + gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); + measurements.posePrior = currentPose; + else + % Generate measurements as the current pose measured in the frame of + % the previous pose + deltaPose = prevPose.between(currentPose); + measurements.gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); + + % Add the factor to the factor graph + gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); + end + prevPose = currentPose; + end else %% Create a random trajectory as ground truth - currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements) + currentVel = [0; 0; 0]; % initial velocity (used to generate IMU measurements) currentPose = Pose3; % initial pose % initial pose - deltaT = 1.0; % amount of time between IMU measurements + deltaT = 0.1; % amount of time between IMU measurements g = [0; 0; 0]; % gravity omegaCoriolis = [0; 0; 0]; % Coriolis @@ -104,9 +107,9 @@ else if includeIMUFactors == 1 currentVelKey = symbol('v', 0); currentBiasKey = symbol('b', 0); - gtValues.insert(currentVelKey, LieVector(vel')); + gtValues.insert(currentVelKey, LieVector(currentVel)); gtValues.insert(currentBiasKey, zeroBias); - gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(vel'), noiseVel)); + gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel)); gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, noiseBias)); end @@ -116,60 +119,71 @@ else gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; - measurements.deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); + gtMeasurements.deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); % "Deduce" ground truth measurements % deltaPose are the gt measurements - save them in some structure - currentPose = currentPose.compose(deltaPose); + currentPose = currentPose.compose(gtMeasurements.deltaPose); gtValues.insert(currentPoseKey, currentPose); % Add the factors to the factor graph - gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); + gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, gtMeasurements.deltaPose, noisePose)); % Add IMU factors if includeIMUFactors == 1 currentVelKey = symbol('v', i); % not used if includeIMUFactors is false currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false - + % create accel and gyro measurements based on - measurements.imu.gyro = gtDeltaMatrix(i, 1:3)./deltaT; + gtMeasurements.imu.gyro = gtDeltaMatrix(i, 1:3)'./deltaT; % acc = (deltaPosition - initialVel * dT) * (2/dt^2) - measurements.imu.accel = (gtDeltaMatrix(i, 4:6) - currentVel.*deltaT).*(2/(deltaT*deltaT)); - % update current velocity - currentVel = gtDeltaMatrix(i,4:6)./deltaT; - imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... - zeroBias, ... + gtMeasurements.imu.accel = (gtDeltaMatrix(i, 4:6)' - currentVel.*deltaT).*(2/(deltaT*deltaT)); + % Initialize preintegration + imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(zeroBias, ... IMU_metadata.AccelerometerSigma.^2 * eye(3), ... IMU_metadata.GyroscopeSigma.^2 * eye(3), ... IMU_metadata.IntegrationSigma.^2 * eye(3)); - imuMeasurement.integrateMeasurement(accel', gyro', deltaT); - gtGraph.add(ImuFactor( ... - currentPoseKey-1, currentVelKey-1, ... - currentPoseKey, currentVelKey, ... + % Preintegrate + imuMeasurement.integrateMeasurement(gtMeasurements.imu.accel, gtMeasurements.imu.gyro, deltaT); + % Add Imu factor + gtGraph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... currentBiasKey-1, imuMeasurement, g, omegaCoriolis)); + % Add between on biases gtGraph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ... noiseModel.Isotropic.Sigma(6, epsBias))); + % Additional prior on zerobias gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ... noiseModel.Isotropic.Sigma(6, epsBias))); - gtValues.insert(currentVelKey, LieVector(vel')); + + % update current velocity + currentVel = gtDeltaMatrix(i,4:6)'./deltaT; + gtValues.insert(currentVelKey, LieVector(currentVel)); + + gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel)); + gtValues.insert(currentBiasKey, zeroBias); end - end + end % end of trajectory length -end +end % end of ground truth creation -gtPoses = Values; -for i=0:trajectoryLength - currentPoseKey = symbol('x', i); - currentPose = gtValues.at(currentPoseKey); - gtPoses.insert(currentPoseKey, currentPose); -end +warning('Additional prior on zerobias') +warning('Additional PriorFactorLieVector on velocities') + +% gtPoses = Values; +% for i=0:trajectoryLength +% currentPoseKey = symbol('x', i); +% currentPose = gtValues.at(currentPoseKey); +% gtPoses.insert(currentPoseKey, currentPose); +% end figure(1) hold on; -plot3DTrajectory(gtPoses, '-r', [], 1, Marginals(gtGraph, gtPoses)); +plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); axis equal +dis('Plotted ground truth') + numMonteCarloRuns = 100; for k=1:numMonteCarloRuns fprintf('Monte Carlo Run %d.\n', k'); @@ -179,8 +193,8 @@ for k=1:numMonteCarloRuns % noisy prior if useRealData == 1 currentPoseKey = symbol('x', 0); - initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario2.Lat(1); gtScenario2.Lon(1); gtScenario2.Alt(1)]); - initialRotation = [gtScenario2.Roll(1); gtScenario2.Pitch(1); gtScenario2.Heading(1)]; + initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]); + initialRotation = [gtScenario.Roll(1); gtScenario.Pitch(1); gtScenario.Heading(1)]; initialPose = Pose3.Expmap([initialRotation; initialPosition] + (noiseVector .* randn(6,1))); % initial noisy pose graph.add(PriorFactorPose3(currentPoseKey, initialPose, noisePose)); else diff --git a/matlab/unstable_examples/+imuSimulator/ct2ENU.m b/matlab/unstable_examples/+imuSimulator/ct2ENU.m new file mode 100644 index 000000000..b7257b664 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/ct2ENU.m @@ -0,0 +1,55 @@ +function [dx,dy,dz]=ct2ENU(dX,dY,dZ,lat,lon) +% CT2LG Converts CT coordinate differences to local geodetic. +% Local origin at lat,lon,h. If lat,lon are vectors, dx,dy,dz +% are referenced to orgin at lat,lon of same index. If +% astronomic lat,lon input, output is in local astronomic +% system. Vectorized in both dx,dy,dz and lat,lon. See also +% LG2CT. +% Version: 2011-02-19 +% Useage: [dx,dy,dz]=ct2lg(dX,dY,dZ,lat,lon) +% Input: dX - vector of X coordinate differences in CT +% dY - vector of Y coordinate differences in CT +% dZ - vector of Z coordinate differences in CT +% lat - lat(s) of local system origin (rad); may be vector +% lon - lon(s) of local system origin (rad); may be vector +% Output: dx - vector of x coordinates in local system (east) +% dy - vector of y coordinates in local system (north) +% dz - vector of z coordinates in local system (up) + +% Copyright (c) 2011, Michael R. Craymer +% All rights reserved. +% Email: mike@craymer.com + +if nargin ~= 5 + warning('Incorrect number of input arguements'); + return +end + +n=length(dX); +if length(lat)==1 + lat=ones(n,1)*lat; + lon=ones(n,1)*lon; +end +R=zeros(3,3,n); + +R(1,1,:)=-sin(lat').*cos(lon'); +R(1,2,:)=-sin(lat').*sin(lon'); +R(1,3,:)=cos(lat'); +R(2,1,:)=sin(lon'); +R(2,2,:)=-cos(lon'); +R(2,3,:)=zeros(1,n); +R(3,1,:)=cos(lat').*cos(lon'); +R(3,2,:)=cos(lat').*sin(lon'); +R(3,3,:)=sin(lat'); + +RR=reshape(R(1,:,:),3,n); +dx_temp=sum(RR'.*[dX dY dZ],2); +RR=reshape(R(2,:,:),3,n); +dy_temp=sum(RR'.*[dX dY dZ],2); +RR=reshape(R(3,:,:),3,n); +dz=sum(RR'.*[dX dY dZ],2); + +dx = -dy_temp; +dy = dx_temp; + + From f38d8d7c83690006c66bb46ef99f707fc3fd02e9 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 10 Apr 2014 22:56:46 -0400 Subject: [PATCH 40/90] completed test infrastructure for simulated and real consistency tests --- .../+imuSimulator/covarianceAnalysisBetween.m | 51 ++++++++++--------- 1 file changed, 28 insertions(+), 23 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index bcf4fc124..5ae792efb 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -28,10 +28,13 @@ noiseBias = noiseModel.Isotropic.Sigma(6, epsBias); %% Between metadata if useRealData == 1 - sigma_ang = 1e-4; sigma_cart = 0.01; + sigma_ang = 1e-1; sigma_cart = 1; else sigma_ang = 1e-2; sigma_cart = 0.1; end +testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart) +folderName = 'results/' + noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); @@ -55,13 +58,13 @@ if useRealData == 1 trajectoryLength = min([length(gtScenario.Lat) trajectoryLength]); for i=1:trajectoryLength - currentPoseKey = symbol('x', i-1); + currentPoseKey = symbol('x', i-1); scenarioInd = subsampleStep * (i-1) + 1 - gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); + gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); % truth in ENU dX = gtECEF(1) - initialPositionECEF(1); dY = gtECEF(2) - initialPositionECEF(2); - dZ = gtECEF(3) - initialPositionECEF(3); + dZ = gtECEF(3) - initialPositionECEF(3); [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); gtPosition = [xlt, ylt, zlt]'; @@ -76,15 +79,15 @@ if useRealData == 1 warning('using identity rotation') gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); measurements.posePrior = currentPose; - else + else % Generate measurements as the current pose measured in the frame of % the previous pose deltaPose = prevPose.between(currentPose); measurements.gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); - + % Add the factor to the factor graph gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); - end + end prevPose = currentPose; end else @@ -182,7 +185,7 @@ hold on; plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); axis equal -dis('Plotted ground truth') +disp('Plotted ground truth') numMonteCarloRuns = 100; for k=1:numMonteCarloRuns @@ -191,24 +194,17 @@ for k=1:numMonteCarloRuns graph = NonlinearFactorGraph; % noisy prior - if useRealData == 1 - currentPoseKey = symbol('x', 0); - initialPosition = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]); - initialRotation = [gtScenario.Roll(1); gtScenario.Pitch(1); gtScenario.Heading(1)]; - initialPose = Pose3.Expmap([initialRotation; initialPosition] + (noiseVector .* randn(6,1))); % initial noisy pose - graph.add(PriorFactorPose3(currentPoseKey, initialPose, noisePose)); - else - currentPoseKey = symbol('x', 0); - noisyDelta = noiseVectorPose .* randn(6,1); - initialPose = Pose3.Expmap(noisyDelta); - graph.add(PriorFactorPose3(currentPoseKey, initialPose, noisePose)); - end + currentPoseKey = symbol('x', 0); + measurements.posePrior = currentPose; + noisyDelta = noiseVectorPose .* randn(6,1); + noisyInitialPose = Pose3.Expmap(noisyDelta); + graph.add(PriorFactorPose3(currentPoseKey, noisyInitialPose, noisePose)); - for i=1:size(gtDeltaMatrix,1) + for i=1:size(measurements.gtDeltaMatrix,1) currentPoseKey = symbol('x', i); % for each measurement: add noise and add to graph - noisyDelta = gtDeltaMatrix(i,:)' + (noiseVectorPose .* randn(6,1)); + noisyDelta = measurements.gtDeltaMatrix(i,:)' + (noiseVectorPose .* randn(6,1)); noisyDeltaPose = Pose3.Expmap(noisyDelta); % Add the factors to the factor graph @@ -225,7 +221,7 @@ for k=1:numMonteCarloRuns marginals = Marginals(graph, estimate); % for each pose in the trajectory - for i=1:size(gtDeltaMatrix,1)+1 + for i=1:size(measurements.gtDeltaMatrix,1)+1 % compute estimation errors currentPoseKey = symbol('x', i-1); gtPosition = gtValues.at(currentPoseKey).translation.vector; @@ -252,12 +248,16 @@ plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof box on set(gca,'Fontsize',16) title('NEES and ANEES'); +%print('-djpeg', horzcat('runs-',testName)); +saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig'); %% figure(1) box on set(gca,'Fontsize',16) title('Ground truth and estimates for each MC runs'); +%print('-djpeg', horzcat('gt-',testName)); +saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig'); %% Let us compute statistics on the overall NEES n = 3; % position vector dimension @@ -282,6 +282,11 @@ plot(r2*ones(size(ANEES,2),1),'k-.'); box on set(gca,'Fontsize',16) title('NEES normalized by dof VS bounds'); +%print('-djpeg', horzcat('ANEES-',testName)); +saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig'); + +logFile = horzcat(folderName,'log-',testName); +save(logFile) %% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4) % the nees for a single experiment (i) is defined as From 97dd2fb9310fce01e9ec2064962d936ea30aab86 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Mon, 14 Apr 2014 14:38:20 -0400 Subject: [PATCH 41/90] Added camera factors to ground truth creation --- .../+imuSimulator/covarianceAnalysisBetween.m | 81 +++++++++++++++---- 1 file changed, 67 insertions(+), 14 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 5ae792efb..a41fda6ba 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -1,6 +1,7 @@ import gtsam.*; % Test GTSAM covariances on a graph with betweenFactors +% Optionally, you can also enable IMU factors and Camera factors % Authors: Luca Carlone, David Jensen % Date: 2014/4/6 @@ -9,10 +10,28 @@ clear all close all %% Configuration -useRealData = 1; % controls whether or not to use the Real data (is available) as the ground truth traj +useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj includeIMUFactors = 0; % if true, IMU type 1 Factors will be generated for the random trajectory -% includeCameraFactors = 0; % not implemented yet -trajectoryLength = 210; % length of the ground truth trajectory +includeCameraFactors = 0; % not implemented yet +trajectoryLength = 10; % length of the ground truth trajectory + +numMonteCarloRuns = 0; + +%% Camera metadata +numberOfLandmarks = 40; % Total number of visual landmarks, used for camera factors +K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration +cameraMeasurementNoiseSigma = 1.0; +cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); + +% Create landmarks +if includeCameraFactors == 1 + for i = 1:numberOfLandmarks + gtLandmarkPoints(i) = Point3( ... + [rand()*20*(trajectoryLength*2.0); ... % uniformly distributed in the x axis along 200% of the trajectory length + randn()*50; ... % normally distributed in the y axis with a sigma of 50 + randn()*50]); % normally distributed in the z axis with a sigma of 50 + end +end %% Imu metadata epsBias = 1e-7; @@ -92,7 +111,7 @@ if useRealData == 1 end else %% Create a random trajectory as ground truth - currentVel = [0; 0; 0]; % initial velocity (used to generate IMU measurements) + currentVel = [0; 0; 0]; % initial velocity (used to generate IMU measurements) currentPose = Pose3; % initial pose % initial pose deltaT = 0.1; % amount of time between IMU measurements g = [0; 0; 0]; % gravity @@ -116,13 +135,19 @@ else gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, noiseBias)); end + if includeCameraFactors == 1 + pointNoiseSigma = 0.1; + pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); + gtGraph.add(PriorFactorPoint3(symbol('p',1), gtLandmarkPoints(1), pointPriorNoise)); + end + for i=1:trajectoryLength currentPoseKey = symbol('x', i); - gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 - gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) - gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; - gtMeasurements.deltaPose = Pose3.Expmap(gtDeltaMatrix(i,:)'); + gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [20 0 0] + gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] + measurements.gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; + gtMeasurements.deltaPose = Pose3.Expmap(measurements.gtDeltaMatrix(i,:)'); % "Deduce" ground truth measurements % deltaPose are the gt measurements - save them in some structure @@ -132,15 +157,15 @@ else % Add the factors to the factor graph gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, gtMeasurements.deltaPose, noisePose)); - % Add IMU factors + %% Add IMU factors if includeIMUFactors == 1 currentVelKey = symbol('v', i); % not used if includeIMUFactors is false currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false % create accel and gyro measurements based on - gtMeasurements.imu.gyro = gtDeltaMatrix(i, 1:3)'./deltaT; + gtMeasurements.imu.gyro = measurements.gtDeltaMatrix(i, 1:3)'./deltaT; % acc = (deltaPosition - initialVel * dT) * (2/dt^2) - gtMeasurements.imu.accel = (gtDeltaMatrix(i, 4:6)' - currentVel.*deltaT).*(2/(deltaT*deltaT)); + gtMeasurements.imu.accel = (measurements.gtDeltaMatrix(i, 4:6)' - currentVel.*deltaT).*(2/(deltaT*deltaT)); % Initialize preintegration imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(zeroBias, ... IMU_metadata.AccelerometerSigma.^2 * eye(3), ... @@ -159,15 +184,43 @@ else noiseModel.Isotropic.Sigma(6, epsBias))); % update current velocity - currentVel = gtDeltaMatrix(i,4:6)'./deltaT; + currentVel = measurements.gtDeltaMatrix(i,4:6)'./deltaT; gtValues.insert(currentVelKey, LieVector(currentVel)); gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel)); gtValues.insert(currentBiasKey, zeroBias); - end + end % end of IMU factor creation + + %% Add Camera factors + if includeCameraFactors == 1 + % Create camera with the current pose and calibration K (specified above) + gtCamera = SimpleCamera(currentPose, K); + % Project landmarks into the camera + numSkipped = 0; + for j = 1:length(gtLandmarkPoints) + landmarkKey = symbol('p', j); + try + Z = gtCamera.project(gtLandmarkPoints(j)); + gtGraph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K)); + catch + % Most likely the point is not within the camera's view, which + % is fine + numSkipped = numSkipped + 1; + end + end + fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped); + end % end of Camera factor creation end % end of trajectory length + %% Add landmark positions to the Values + if includeCameraFactors == 1 + for j = 1:length(gtLandmarkPoints) + landmarkKey = symbol('p', j); + gtValues.insert(landmarkKey, gtLandmarkPoints(j)); + end + end + end % end of ground truth creation warning('Additional prior on zerobias') @@ -182,12 +235,12 @@ warning('Additional PriorFactorLieVector on velocities') figure(1) hold on; +plot3DPoints(gtValues); plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); axis equal disp('Plotted ground truth') -numMonteCarloRuns = 100; for k=1:numMonteCarloRuns fprintf('Monte Carlo Run %d.\n', k'); % create a new graph From f63db8859b90b7e7710a24123fb8edc2dca98e9d Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 15 Apr 2014 10:56:07 -0400 Subject: [PATCH 42/90] addind wrapper for smart factors --- gtsam_unstable/gtsam_unstable.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 80ee41b22..7ccde30a4 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -376,6 +376,20 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; +//#include +//template +//virtual class SmartProjectionPoseFactor: public SmartProjectionFactor { + +// SmartProjectionPoseFactor(const double rankTol, const double linThreshold, +// const bool manageDegeneracy, const bool enableEPI, const POSE& body_P_sensor); +// +// void add(const Point2 measured_i, const Key poseKey_i, const SharedNoiseModel noise_i, +// const CALIBRATION* K_i); +// +// // enabling serialization functionality +// void serialize() const; +}; + #include template From 75b042bbcd1336c382b3d7e2d05366df50f35dfe Mon Sep 17 00:00:00 2001 From: djensen3 Date: Tue, 15 Apr 2014 11:05:43 -0400 Subject: [PATCH 43/90] Working on IMU factors. made changes to noise --- .../+imuSimulator/covarianceAnalysisBetween.m | 45 ++++++++++++------- 1 file changed, 30 insertions(+), 15 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index a41fda6ba..7c288784b 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -11,14 +11,16 @@ close all %% Configuration useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj -includeIMUFactors = 0; % if true, IMU type 1 Factors will be generated for the random trajectory -includeCameraFactors = 0; % not implemented yet -trajectoryLength = 10; % length of the ground truth trajectory + +includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the random trajectory +includeCameraFactors = 0; % not fully implemented yet + +trajectoryLength = 4; % length of the ground truth trajectory numMonteCarloRuns = 0; %% Camera metadata -numberOfLandmarks = 40; % Total number of visual landmarks, used for camera factors +numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration cameraMeasurementNoiseSigma = 1.0; cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); @@ -27,14 +29,14 @@ cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigm if includeCameraFactors == 1 for i = 1:numberOfLandmarks gtLandmarkPoints(i) = Point3( ... - [rand()*20*(trajectoryLength*2.0); ... % uniformly distributed in the x axis along 200% of the trajectory length - randn()*50; ... % normally distributed in the y axis with a sigma of 50 - randn()*50]); % normally distributed in the z axis with a sigma of 50 + [rand()*20*(trajectoryLength*1.2) + 15*20; ... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses + randn()*20; ... % normally distributed in the y axis with a sigma of 20 + randn()*20]); % normally distributed in the z axis with a sigma of 20 end end %% Imu metadata -epsBias = 1e-7; +epsBias = 1e-20; % was 1e-7 zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.GyroscopeSigma = 1e-7; @@ -42,7 +44,7 @@ IMU_metadata.IntegrationSigma = 1e-10; IMU_metadata.BiasAccelerometerSigma = epsBias; IMU_metadata.BiasGyroscopeSigma = epsBias; IMU_metadata.BiasAccOmegaInit = epsBias; -noiseVel = noiseModel.Isotropic.Sigma(3, 0.1); +noiseVel = noiseModel.Isotropic.Sigma(3, 1e-10); % was 0.1 noiseBias = noiseModel.Isotropic.Sigma(6, epsBias); %% Between metadata @@ -56,6 +58,7 @@ folderName = 'results/' noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); +%noisePose = noiseModel.Isotropic.Sigma(6, 1e-3); %% Create ground truth trajectory gtValues = Values; @@ -120,11 +123,17 @@ else unsmooth_DP = 0.5; % controls smoothness on translation norm unsmooth_DR = 0.1; % controls smoothness on rotation norm + unsmooth_DP = 0; + unsmooth_DR = 0; + fprintf('\nCreating a random ground truth trajectory\n'); %% Add priors currentPoseKey = symbol('x', 0); gtValues.insert(currentPoseKey, currentPose); - gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); + % NOSIE ON PRIOR WAS TOO HIGH? Changing this fixed the indeterminant + % linear system error + gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noiseModel.Isotropic.Sigma(6, 1e-3))); + %gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose); if includeIMUFactors == 1 currentVelKey = symbol('v', 0); @@ -155,7 +164,7 @@ else gtValues.insert(currentPoseKey, currentPose); % Add the factors to the factor graph - gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, gtMeasurements.deltaPose, noisePose)); + %gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, gtMeasurements.deltaPose, noisePose)); %% Add IMU factors if includeIMUFactors == 1 @@ -167,7 +176,8 @@ else % acc = (deltaPosition - initialVel * dT) * (2/dt^2) gtMeasurements.imu.accel = (measurements.gtDeltaMatrix(i, 4:6)' - currentVel.*deltaT).*(2/(deltaT*deltaT)); % Initialize preintegration - imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(zeroBias, ... + imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... + zeroBias, ... IMU_metadata.AccelerometerSigma.^2 * eye(3), ... IMU_metadata.GyroscopeSigma.^2 * eye(3), ... IMU_metadata.IntegrationSigma.^2 * eye(3)); @@ -187,7 +197,7 @@ else currentVel = measurements.gtDeltaMatrix(i,4:6)'./deltaT; gtValues.insert(currentVelKey, LieVector(currentVel)); - gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel)); + %gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel)); gtValues.insert(currentBiasKey, zeroBias); end % end of IMU factor creation @@ -202,6 +212,8 @@ else landmarkKey = symbol('p', j); try Z = gtCamera.project(gtLandmarkPoints(j)); + %% TO-DO probably want to do some type of filtering on the measurement values, because + % they might not all be valid gtGraph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K)); catch % Most likely the point is not within the camera's view, which @@ -209,7 +221,7 @@ else numSkipped = numSkipped + 1; end end - fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped); + %fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped); end % end of Camera factor creation end % end of trajectory length @@ -223,6 +235,9 @@ else end % end of ground truth creation +gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); +gtValues.print(sprintf('\nGround Truth Values:\n ')); + warning('Additional prior on zerobias') warning('Additional PriorFactorLieVector on velocities') @@ -261,7 +276,7 @@ for k=1:numMonteCarloRuns noisyDeltaPose = Pose3.Expmap(noisyDelta); % Add the factors to the factor graph - graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose)); + %graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose)); end % optimize From e232b6085bfccbca04b2f991f27f9159e50266a9 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 15 Apr 2014 11:06:38 -0400 Subject: [PATCH 44/90] wrapping smart factors --- gtsam_unstable/gtsam_unstable.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 7ccde30a4..d87e71048 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -388,7 +388,7 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { // // // enabling serialization functionality // void serialize() const; -}; +//}; #include From 8367d45e48effcd6b5596476f71811801eb3db66 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 15 Apr 2014 12:12:39 -0400 Subject: [PATCH 45/90] improved matlab code for consistency check --- .../+imuSimulator/covarianceAnalysisBetween.m | 154 ++++++++---------- 1 file changed, 70 insertions(+), 84 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 7c288784b..125f44808 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -12,12 +12,15 @@ close all %% Configuration useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj -includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the random trajectory -includeCameraFactors = 0; % not fully implemented yet +%options. +includeBetweenFactors = 1; % if true, BetweenFactors will be generated between consecutive poses +includeIMUFactors = 0; % if true, IMU type 1 Factors will be generated for the random trajectory +includeCameraFactors = 0; % not fully implemented yet trajectoryLength = 4; % length of the ground truth trajectory +subsampleStep = 20; -numMonteCarloRuns = 0; +numMonteCarloRuns = 2; %% Camera metadata numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors @@ -29,44 +32,39 @@ cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigm if includeCameraFactors == 1 for i = 1:numberOfLandmarks gtLandmarkPoints(i) = Point3( ... - [rand()*20*(trajectoryLength*1.2) + 15*20; ... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses - randn()*20; ... % normally distributed in the y axis with a sigma of 20 - randn()*20]); % normally distributed in the z axis with a sigma of 20 + [rand()*20*(trajectoryLength*1.2) + 15*20; ... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses + randn()*20; ... % normally distributed in the y axis with a sigma of 20 + randn()*20]); % normally distributed in the z axis with a sigma of 20 end end %% Imu metadata -epsBias = 1e-20; % was 1e-7 +epsBias = 1e-10; % was 1e-7 zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.GyroscopeSigma = 1e-7; -IMU_metadata.IntegrationSigma = 1e-10; +IMU_metadata.IntegrationSigma = 1e-4; IMU_metadata.BiasAccelerometerSigma = epsBias; IMU_metadata.BiasGyroscopeSigma = epsBias; IMU_metadata.BiasAccOmegaInit = epsBias; -noiseVel = noiseModel.Isotropic.Sigma(3, 1e-10); % was 0.1 +noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 noiseBias = noiseModel.Isotropic.Sigma(6, epsBias); +noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4); %% Between metadata -if useRealData == 1 - sigma_ang = 1e-1; sigma_cart = 1; -else - sigma_ang = 1e-2; sigma_cart = 0.1; -end -testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart) -folderName = 'results/' - +sigma_ang = 1e-2; sigma_cart = 1; noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); %noisePose = noiseModel.Isotropic.Sigma(6, 1e-3); +%% Set log files +testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart) +folderName = 'results/' + %% Create ground truth trajectory gtValues = Values; -gtGraph = NonlinearFactorGraph; if useRealData == 1 - subsampleStep = 20; - %% Create a ground truth trajectory from Real data (if available) fprintf('\nUsing real data as ground truth\n'); gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',... @@ -95,22 +93,6 @@ if useRealData == 1 % Add values gtValues.insert(currentPoseKey, currentPose); - - if i==1 % first time step, add priors - warning('roll-pitch-yaw is different from Rodriguez') - warning('using identity rotation') - gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); - measurements.posePrior = currentPose; - else - % Generate measurements as the current pose measured in the frame of - % the previous pose - deltaPose = prevPose.between(currentPose); - measurements.gtDeltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); - - % Add the factor to the factor graph - gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, deltaPose, noisePose)); - end - prevPose = currentPose; end else %% Create a random trajectory as ground truth @@ -123,32 +105,9 @@ else unsmooth_DP = 0.5; % controls smoothness on translation norm unsmooth_DR = 0.1; % controls smoothness on rotation norm - unsmooth_DP = 0; - unsmooth_DR = 0; - fprintf('\nCreating a random ground truth trajectory\n'); - %% Add priors currentPoseKey = symbol('x', 0); gtValues.insert(currentPoseKey, currentPose); - % NOSIE ON PRIOR WAS TOO HIGH? Changing this fixed the indeterminant - % linear system error - gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noiseModel.Isotropic.Sigma(6, 1e-3))); - %gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose); - - if includeIMUFactors == 1 - currentVelKey = symbol('v', 0); - currentBiasKey = symbol('b', 0); - gtValues.insert(currentVelKey, LieVector(currentVel)); - gtValues.insert(currentBiasKey, zeroBias); - gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel)); - gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, noiseBias)); - end - - if includeCameraFactors == 1 - pointNoiseSigma = 0.1; - pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); - gtGraph.add(PriorFactorPoint3(symbol('p',1), gtLandmarkPoints(1), pointPriorNoise)); - end for i=1:trajectoryLength currentPoseKey = symbol('x', i); @@ -162,9 +121,49 @@ else % deltaPose are the gt measurements - save them in some structure currentPose = currentPose.compose(gtMeasurements.deltaPose); gtValues.insert(currentPoseKey, currentPose); + end +end +% we computed gtValues + +gtGraph = NonlinearFactorGraph; +for i=0:trajectoryLength + + currentPoseKey = symbol('x', i); + currentPose = gtValues.at(currentPoseKey); + + if i==0 + %% first time step, add priors + warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') + warning('using identity rotation') + gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); + measurements.posePrior = currentPose; - % Add the factors to the factor graph - %gtGraph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, gtMeasurements.deltaPose, noisePose)); + if includeIMUFactors == 1 + currentVelKey = symbol('v', 0); + currentBiasKey = symbol('b', 0); + gtValues.insert(currentVelKey, LieVector(currentVel)); + gtValues.insert(currentBiasKey, zeroBias); + gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel)); + gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, noisePriorBias)); + end + + if includeCameraFactors == 1 + pointNoiseSigma = 0.1; + pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); + gtGraph.add(PriorFactorPoint3(symbol('p',1), gtLandmarkPoints(1), pointPriorNoise)); + end + + else + %% other factors + if includeBetweenFactors == 1 + prevPoseKey = symbol('x', i-1); + prevPose = gtValues.at(prevPoseKey); + deltaPose = prevPose.between(currentPose); + measurements.gtDeltaMatrix(i,:) = Pose3.Logmap(deltaPose); + + % Add the factor to the factor graph + gtGraph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noisePose)); + end %% Add IMU factors if includeIMUFactors == 1 @@ -196,9 +195,7 @@ else % update current velocity currentVel = measurements.gtDeltaMatrix(i,4:6)'./deltaT; gtValues.insert(currentVelKey, LieVector(currentVel)); - - %gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel)); - + gtValues.insert(currentBiasKey, zeroBias); end % end of IMU factor creation @@ -223,31 +220,17 @@ else end %fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped); end % end of Camera factor creation - end % end of trajectory length + + end % end of else - %% Add landmark positions to the Values - if includeCameraFactors == 1 - for j = 1:length(gtLandmarkPoints) - landmarkKey = symbol('p', j); - gtValues.insert(landmarkKey, gtLandmarkPoints(j)); - end - end - -end % end of ground truth creation +end % end of for over trajectory -gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); -gtValues.print(sprintf('\nGround Truth Values:\n ')); +%gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); +%gtValues.print(sprintf('\nGround Truth Values:\n ')); warning('Additional prior on zerobias') warning('Additional PriorFactorLieVector on velocities') -% gtPoses = Values; -% for i=0:trajectoryLength -% currentPoseKey = symbol('x', i); -% currentPose = gtValues.at(currentPoseKey); -% gtPoses.insert(currentPoseKey, currentPose); -% end - figure(1) hold on; plot3DPoints(gtValues); @@ -269,6 +252,7 @@ for k=1:numMonteCarloRuns graph.add(PriorFactorPose3(currentPoseKey, noisyInitialPose, noisePose)); for i=1:size(measurements.gtDeltaMatrix,1) + i currentPoseKey = symbol('x', i); % for each measurement: add noise and add to graph @@ -276,9 +260,11 @@ for k=1:numMonteCarloRuns noisyDeltaPose = Pose3.Expmap(noisyDelta); % Add the factors to the factor graph - %graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose)); + graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose)); end + graph.print('graph') + % optimize optimizer = GaussNewtonOptimizer(graph, gtValues); estimate = optimizer.optimize(); From b85ebb501d1d0244ff176109af948f79246ed0db Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 16 Apr 2014 15:01:12 -0400 Subject: [PATCH 46/90] restructuring code to utilize functions and reduce size of primary script --- .../+imuSimulator/covarianceAnalysisBetween.m | 233 ++++-------------- .../covarianceAnalysisCreateFactorGraph.m | 110 +++++++++ .../covarianceAnalysisCreateTrajectory.m | 112 +++++++++ 3 files changed, 270 insertions(+), 185 deletions(-) create mode 100644 matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m create mode 100644 matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 125f44808..22ca92164 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -10,15 +10,12 @@ clear all close all %% Configuration -useRealData = 0; % controls whether or not to use the Real data (is available) as the ground truth traj - -%options. -includeBetweenFactors = 1; % if true, BetweenFactors will be generated between consecutive poses -includeIMUFactors = 0; % if true, IMU type 1 Factors will be generated for the random trajectory -includeCameraFactors = 0; % not fully implemented yet - -trajectoryLength = 4; % length of the ground truth trajectory -subsampleStep = 20; +options.useRealData = 0; % controls whether or not to use the real data (if available) as the ground truth traj +options.includeBetweenFactors = 1; % if true, BetweenFactors will be generated between consecutive poses +options.includeIMUFactors = 0; % if true, IMU type 1 Factors will be generated for the trajectory +options.includeCameraFactors = 0; % not fully implemented yet +options.trajectoryLength = 4; % length of the ground truth trajectory +options.subsampleStep = 20; numMonteCarloRuns = 2; @@ -29,26 +26,29 @@ cameraMeasurementNoiseSigma = 1.0; cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); % Create landmarks -if includeCameraFactors == 1 +if options.includeCameraFactors == 1 for i = 1:numberOfLandmarks gtLandmarkPoints(i) = Point3( ... - [rand()*20*(trajectoryLength*1.2) + 15*20; ... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses + ... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses + [rand()*20*(options.trajectoryLength*1.2) + 15*20; ... randn()*20; ... % normally distributed in the y axis with a sigma of 20 randn()*20]); % normally distributed in the z axis with a sigma of 20 end end %% Imu metadata -epsBias = 1e-10; % was 1e-7 -zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); -IMU_metadata.AccelerometerSigma = 1e-5; -IMU_metadata.GyroscopeSigma = 1e-7; -IMU_metadata.IntegrationSigma = 1e-4; -IMU_metadata.BiasAccelerometerSigma = epsBias; -IMU_metadata.BiasGyroscopeSigma = epsBias; -IMU_metadata.BiasAccOmegaInit = epsBias; +metadata.imu.epsBias = 1e-10; % was 1e-7 +metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); +metadata.imu.AccelerometerSigma = 1e-5; +metadata.imu.GyroscopeSigma = 1e-7; +metadata.imu.IntegrationSigma = 1e-4; +metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; +metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; +metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias; +metadata.imu.g = [0;0;0]; +metadata.imu.omegaCoriolis = [0;0;0]; noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 -noiseBias = noiseModel.Isotropic.Sigma(6, epsBias); +noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4); %% Between metadata @@ -61,170 +61,32 @@ noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart) folderName = 'results/' -%% Create ground truth trajectory -gtValues = Values; +%% Create ground truth trajectory and measurements +[gtValues, gtMeasurements] = imuSimulator.covarianceAnalysisCreateTrajectory(options, metadata); -if useRealData == 1 - %% Create a ground truth trajectory from Real data (if available) - fprintf('\nUsing real data as ground truth\n'); - gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',... - 'VEast', 'VNorth', 'VUp'); - - Org_lat = gtScenario.Lat(1); - Org_lon = gtScenario.Lon(1); - initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]); - - % Limit the trajectory length - trajectoryLength = min([length(gtScenario.Lat) trajectoryLength]); - - for i=1:trajectoryLength - currentPoseKey = symbol('x', i-1); - scenarioInd = subsampleStep * (i-1) + 1 - gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); - % truth in ENU - dX = gtECEF(1) - initialPositionECEF(1); - dY = gtECEF(2) - initialPositionECEF(2); - dZ = gtECEF(3) - initialPositionECEF(3); - [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); - - gtPosition = [xlt, ylt, zlt]'; - gtRotation = Rot3; % Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); - currentPose = Pose3(gtRotation, Point3(gtPosition)); - - % Add values - gtValues.insert(currentPoseKey, currentPose); - end -else - %% Create a random trajectory as ground truth - currentVel = [0; 0; 0]; % initial velocity (used to generate IMU measurements) - currentPose = Pose3; % initial pose % initial pose - deltaT = 0.1; % amount of time between IMU measurements - g = [0; 0; 0]; % gravity - omegaCoriolis = [0; 0; 0]; % Coriolis - - unsmooth_DP = 0.5; % controls smoothness on translation norm - unsmooth_DR = 0.1; % controls smoothness on rotation norm - - fprintf('\nCreating a random ground truth trajectory\n'); - currentPoseKey = symbol('x', 0); - gtValues.insert(currentPoseKey, currentPose); - - for i=1:trajectoryLength - currentPoseKey = symbol('x', i); - - gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [20 0 0] - gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] - measurements.gtDeltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; - gtMeasurements.deltaPose = Pose3.Expmap(measurements.gtDeltaMatrix(i,:)'); - - % "Deduce" ground truth measurements - % deltaPose are the gt measurements - save them in some structure - currentPose = currentPose.compose(gtMeasurements.deltaPose); - gtValues.insert(currentPoseKey, currentPose); - end -end -% we computed gtValues +%% Create ground truth graph +% Set up noise models +gtNoiseModels.noisePose = noisePose; +gtNoiseModels.noiseVel = noiseVel; +gtNoiseModels.noiseBias = noiseBias; +gtNoiseModels.noisePriorPose = noisePose; +gtNoiseModels.noisePriorBias = noisePriorBias; -gtGraph = NonlinearFactorGraph; -for i=0:trajectoryLength - - currentPoseKey = symbol('x', i); - currentPose = gtValues.at(currentPoseKey); +% Set measurement noise to 0, because this is ground truth +gtMeasurementNoise.poseNoiseVector = [0 0 0 0 0 0]; +gtMeasurementNoise.imu.accelNoiseVector = [0 0 0]; +gtMeasurementNoise.imu.gyroNoiseVector = [0 0 0]; +gtMeasurementNoise.cameraPixelNoiseVector = [0 0]; - if i==0 - %% first time step, add priors - warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') - warning('using identity rotation') - gtGraph.add(PriorFactorPose3(currentPoseKey, currentPose, noisePose)); - measurements.posePrior = currentPose; - - if includeIMUFactors == 1 - currentVelKey = symbol('v', 0); - currentBiasKey = symbol('b', 0); - gtValues.insert(currentVelKey, LieVector(currentVel)); - gtValues.insert(currentBiasKey, zeroBias); - gtGraph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseVel)); - gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, noisePriorBias)); - end - - if includeCameraFactors == 1 - pointNoiseSigma = 0.1; - pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); - gtGraph.add(PriorFactorPoint3(symbol('p',1), gtLandmarkPoints(1), pointPriorNoise)); - end - - else - %% other factors - if includeBetweenFactors == 1 - prevPoseKey = symbol('x', i-1); - prevPose = gtValues.at(prevPoseKey); - deltaPose = prevPose.between(currentPose); - measurements.gtDeltaMatrix(i,:) = Pose3.Logmap(deltaPose); - - % Add the factor to the factor graph - gtGraph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noisePose)); - end - - %% Add IMU factors - if includeIMUFactors == 1 - currentVelKey = symbol('v', i); % not used if includeIMUFactors is false - currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false - - % create accel and gyro measurements based on - gtMeasurements.imu.gyro = measurements.gtDeltaMatrix(i, 1:3)'./deltaT; - % acc = (deltaPosition - initialVel * dT) * (2/dt^2) - gtMeasurements.imu.accel = (measurements.gtDeltaMatrix(i, 4:6)' - currentVel.*deltaT).*(2/(deltaT*deltaT)); - % Initialize preintegration - imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... - zeroBias, ... - IMU_metadata.AccelerometerSigma.^2 * eye(3), ... - IMU_metadata.GyroscopeSigma.^2 * eye(3), ... - IMU_metadata.IntegrationSigma.^2 * eye(3)); - % Preintegrate - imuMeasurement.integrateMeasurement(gtMeasurements.imu.accel, gtMeasurements.imu.gyro, deltaT); - % Add Imu factor - gtGraph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... - currentBiasKey-1, imuMeasurement, g, omegaCoriolis)); - % Add between on biases - gtGraph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ... - noiseModel.Isotropic.Sigma(6, epsBias))); - % Additional prior on zerobias - gtGraph.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ... - noiseModel.Isotropic.Sigma(6, epsBias))); - - % update current velocity - currentVel = measurements.gtDeltaMatrix(i,4:6)'./deltaT; - gtValues.insert(currentVelKey, LieVector(currentVel)); - - gtValues.insert(currentBiasKey, zeroBias); - end % end of IMU factor creation - - %% Add Camera factors - if includeCameraFactors == 1 - % Create camera with the current pose and calibration K (specified above) - gtCamera = SimpleCamera(currentPose, K); - % Project landmarks into the camera - numSkipped = 0; - for j = 1:length(gtLandmarkPoints) - landmarkKey = symbol('p', j); - try - Z = gtCamera.project(gtLandmarkPoints(j)); - %% TO-DO probably want to do some type of filtering on the measurement values, because - % they might not all be valid - gtGraph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K)); - catch - % Most likely the point is not within the camera's view, which - % is fine - numSkipped = numSkipped + 1; - end - end - %fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped); - end % end of Camera factor creation - - end % end of else - -end % end of for over trajectory +[gtGraph, gtValues] = imuSimulator.covarianceAnalysisCreateFactorGraph( ... + gtMeasurements, ... % ground truth measurements + gtValues, ... % ground truth Values + gtNoiseModels, ... % noise models to use in this graph + gtMeasurementNoise, ... % noise to apply to measurements + options, ... % options for the graph (e.g. which factors to include) + metadata); % misc data necessary for factor creation +%% Display, printing, and plotting of ground truth %gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); %gtValues.print(sprintf('\nGround Truth Values:\n ')); @@ -239,6 +101,7 @@ axis equal disp('Plotted ground truth') +%% Monte Carlo Runs for k=1:numMonteCarloRuns fprintf('Monte Carlo Run %d.\n', k'); % create a new graph @@ -246,17 +109,17 @@ for k=1:numMonteCarloRuns % noisy prior currentPoseKey = symbol('x', 0); - measurements.posePrior = currentPose; + currentPose = gtValues.at(currentPoseKey); + gtMeasurements.posePrior = currentPose; noisyDelta = noiseVectorPose .* randn(6,1); noisyInitialPose = Pose3.Expmap(noisyDelta); graph.add(PriorFactorPose3(currentPoseKey, noisyInitialPose, noisePose)); - for i=1:size(measurements.gtDeltaMatrix,1) - i + for i=1:size(gtMeasurements.deltaMatrix,1) currentPoseKey = symbol('x', i); % for each measurement: add noise and add to graph - noisyDelta = measurements.gtDeltaMatrix(i,:)' + (noiseVectorPose .* randn(6,1)); + noisyDelta = gtMeasurements.deltaMatrix(i,:)' + (noiseVectorPose .* randn(6,1)); noisyDeltaPose = Pose3.Expmap(noisyDelta); % Add the factors to the factor graph @@ -275,7 +138,7 @@ for k=1:numMonteCarloRuns marginals = Marginals(graph, estimate); % for each pose in the trajectory - for i=1:size(measurements.gtDeltaMatrix,1)+1 + for i=1:size(gtMeasurements.deltaMatrix,1)+1 % compute estimation errors currentPoseKey = symbol('x', i-1); gtPosition = gtValues.at(currentPoseKey).translation.vector; diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m new file mode 100644 index 000000000..5ca14021e --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -0,0 +1,110 @@ +function [ graph, values ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) +% Create a factor graph based on provided measurements, values, and noises. +% Used for covariance analysis scripts. +% 'options' contains fields for including various factor types. +% 'metadata' is a storage variable for miscellaneous factor-specific values +% Authors: Luca Carlone, David Jensen +% Date: 2014/04/16 + +import gtsam.*; + +graph = NonlinearFactorGraph; + +% Iterate through the trajectory +for i=0:size(measurements.deltaMatrix, 1); + % Get the current pose + currentPoseKey = symbol('x', i); + currentPose = values.at(currentPoseKey); + + if i==0 + %% first time step, add priors + warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') + warning('using identity rotation') + graph.add(PriorFactorPose3(currentPoseKey, currentPose, noiseModels.noisePose)); + measurements.posePrior = currentPose; + + if options.includeIMUFactors == 1 + currentVelKey = symbol('v', 0); + currentBiasKey = symbol('b', 0); + currentVel = [0; 0; 0]; + values.insert(currentVelKey, LieVector(currentVel)); + values.insert(currentBiasKey, metadata.imu.zeroBias); + graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); + graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, noiseModels.noisePriorBias)); + end + + if options.includeCameraFactors == 1 + pointNoiseSigma = 0.1; + pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); + graph.add(PriorFactorPoint3(symbol('p',1), gtLandmarkPoints(1), pointPriorNoise)); + end + + else + prevPoseKey = symbol('x', i-1); + + %% Add Between factors + if options.includeBetweenFactors == 1 + % Create the noisy pose estimate + deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)' ... + + (measurementNoise.poseNoiseVector' .* randn(6,1))); % added noise + % Add the between factor to the graph + graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose)); + end % end of Between pose factor creation + + %% Add IMU factors + if options.includeIMUFactors == 1 + % Update keys + currentVelKey = symbol('v', i); % not used if includeIMUFactors is false + currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false + % Generate IMU measurements with noise + imuAccel = measurements.imu.accel(i,:)' ... + + (measurementNoise.imu.accelNoiseVector' .* randn(3,1)); % added noise + imuGyro = measurements.imu.gyro(i,:)' ... + + (measurementNoise.imu.gyroNoiseVector' .* randn(3,1)); % added noise + % Initialize preintegration + imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... + metadata.imu.zeroBias, ... + metadata.imu.AccelerometerSigma.^2 * eye(3), ... + metadata.imu.GyroscopeSigma.^2 * eye(3), ... + metadata.imu.IntegrationSigma.^2 * eye(3)); + % Preintegrate + imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); + % Add Imu factor + graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... + currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); + % Add between factor on biases + graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... + noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + % Additional prior on zerobias + graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ... + noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + end % end of IMU factor creation + + %% Add Camera factors - UNDER CONSTRUCTION !!!! - + if options.includeCameraFactors == 1 + % Create camera with the current pose and calibration K (specified above) + gtCamera = SimpleCamera(currentPose, K); + % Project landmarks into the camera + numSkipped = 0; + for j = 1:length(gtLandmarkPoints) + landmarkKey = symbol('p', j); + try + Z = gtCamera.project(gtLandmarkPoints(j)); + %% TO-DO probably want to do some type of filtering on the measurement values, because + % they might not all be valid + graph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K)); + catch + % Most likely the point is not within the camera's view, which + % is fine + numSkipped = numSkipped + 1; + end + end + %fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped); + end % end of Camera factor creation + + end % end of else + +end % end of for over trajectory + +end + diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m new file mode 100644 index 000000000..cf0214927 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -0,0 +1,112 @@ +function [ values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata ) +% Create a trajectory for running covariance analysis scripts. +% 'options' contains fields for including various factor types and setting trajectory length +% 'metadata' is a storage variable for miscellaneous factor-specific values +% Authors: Luca Carlone, David Jensen +% Date: 2014/04/16 + +import gtsam.*; + +values = Values; + +if options.useRealData == 1 + %% Create a ground truth trajectory from Real data (if available) + fprintf('\nUsing real data as ground truth\n'); + gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',... + 'VEast', 'VNorth', 'VUp'); + + Org_lat = gtScenario.Lat(1); + Org_lon = gtScenario.Lon(1); + initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]); + + % Limit the trajectory length + options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength+1]); + + for i=1:options.trajectoryLength+1 + % Update the pose key + currentPoseKey = symbol('x', i-1); + + % Generate the current pose + scenarioInd = options.subsampleStep * (i-1) + 1 + gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); + % truth in ENU + dX = gtECEF(1) - initialPositionECEF(1); + dY = gtECEF(2) - initialPositionECEF(2); + dZ = gtECEF(3) - initialPositionECEF(3); + [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); + + gtPosition = [xlt, ylt, zlt]'; + gtRotation = Rot3; % Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); + currentPose = Pose3(gtRotation, Point3(gtPosition)); + + % Add values + values.insert(currentPoseKey, currentPose); + + % Generate the measurement. The first pose is considered the prior, so + % it has no measurement + if i > 1 + prevPose = values.at(currentPoseKey - 1); + deltaPose = prevPose.between(currentPose); + measurements.deltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); + end + end +else + %% Create a random trajectory as ground truth + currentPose = Pose3; % initial pose % initial pose + + unsmooth_DP = 0.5; % controls smoothness on translation norm + unsmooth_DR = 0.1; % controls smoothness on rotation norm + + fprintf('\nCreating a random ground truth trajectory\n'); + currentPoseKey = symbol('x', 0); + values.insert(currentPoseKey, currentPose); + + for i=1:options.trajectoryLength + % Update the pose key + currentPoseKey = symbol('x', i); + + % Generate the new measurements + gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [20 0 0] + gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] + measurements.deltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; + + % Create the next pose + deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)'); + currentPose = currentPose.compose(deltaPose); + + % Add the current pose as a value + values.insert(currentPoseKey, currentPose); + end % end of random trajectory creation +end % end of else + +%% Create IMU measurements and Values for the trajectory +if options.includeIMUFactors == 1 +currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements) +deltaT = 0.1; % amount of time between IMU measurements + +% Iterate over the deltaMatrix to generate appropriate IMU measurements +for i = 1:size(measurements.deltaMatrix, 1) + % Update Keys + currentVelKey = symbol('v', i); + currentBiasKey = symbol('b', i); + + measurements.imu.deltaT(i) = deltaT; + + % create accel and gyro measurements based on + measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i); + + % acc = (deltaPosition - initialVel * dT) * (2/dt^2) + measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ... + - currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i))); + + % Update velocity + currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i); + + % Add Values: velocity and bias + values.insert(currentVelKey, LieVector(currentVel')); + values.insert(currentBiasKey, metadata.imu.zeroBias); +end +end % end of IMU measurements + +end + From 1432fb773bab6de8335775398ab25afd2cfbf06d Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 16 Apr 2014 15:25:05 -0400 Subject: [PATCH 47/90] minor changes --- .../+imuSimulator/covarianceAnalysisBetween.m | 22 ++++++++--- .../covarianceAnalysisCreateTrajectory.m | 38 +++++++++---------- 2 files changed, 35 insertions(+), 25 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 22ca92164..4be7678c1 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -12,7 +12,7 @@ close all %% Configuration options.useRealData = 0; % controls whether or not to use the real data (if available) as the ground truth traj options.includeBetweenFactors = 1; % if true, BetweenFactors will be generated between consecutive poses -options.includeIMUFactors = 0; % if true, IMU type 1 Factors will be generated for the trajectory +options.includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the trajectory options.includeCameraFactors = 0; % not fully implemented yet options.trajectoryLength = 4; % length of the ground truth trajectory options.subsampleStep = 20; @@ -78,7 +78,7 @@ gtMeasurementNoise.imu.accelNoiseVector = [0 0 0]; gtMeasurementNoise.imu.gyroNoiseVector = [0 0 0]; gtMeasurementNoise.cameraPixelNoiseVector = [0 0]; -[gtGraph, gtValues] = imuSimulator.covarianceAnalysisCreateFactorGraph( ... +gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... gtMeasurements, ... % ground truth measurements gtValues, ... % ground truth Values gtNoiseModels, ... % noise models to use in this graph @@ -87,8 +87,8 @@ gtMeasurementNoise.cameraPixelNoiseVector = [0 0]; metadata); % misc data necessary for factor creation %% Display, printing, and plotting of ground truth -%gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); -%gtValues.print(sprintf('\nGround Truth Values:\n ')); +gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); +gtValues.print(sprintf('\nGround Truth Values:\n ')); warning('Additional prior on zerobias') warning('Additional PriorFactorLieVector on velocities') @@ -104,6 +104,7 @@ disp('Plotted ground truth') %% Monte Carlo Runs for k=1:numMonteCarloRuns fprintf('Monte Carlo Run %d.\n', k'); + % create a new graph graph = NonlinearFactorGraph; @@ -125,8 +126,17 @@ for k=1:numMonteCarloRuns % Add the factors to the factor graph graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose)); end - - graph.print('graph') + +% graph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... +% gtMeasurements, ... % ground truth measurements +% gtValues, ... % ground truth Values +% gtNoiseModels, ... % noise models to use in this graph +% gtMeasurementNoise, ... % noise to apply to measurements +% options, ... % options for the graph (e.g. which factors to include) +% metadata); % misc data necessary for factor creation + + + %graph.print('graph') % optimize optimizer = GaussNewtonOptimizer(graph, gtValues); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index cf0214927..6a6c3b594 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -81,31 +81,31 @@ end % end of else %% Create IMU measurements and Values for the trajectory if options.includeIMUFactors == 1 -currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements) -deltaT = 0.1; % amount of time between IMU measurements +currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements) +deltaT = 0.1; % amount of time between IMU measurements -% Iterate over the deltaMatrix to generate appropriate IMU measurements -for i = 1:size(measurements.deltaMatrix, 1) - % Update Keys - currentVelKey = symbol('v', i); - currentBiasKey = symbol('b', i); + % Iterate over the deltaMatrix to generate appropriate IMU measurements + for i = 1:size(measurements.deltaMatrix, 1) + % Update Keys + currentVelKey = symbol('v', i); + currentBiasKey = symbol('b', i); - measurements.imu.deltaT(i) = deltaT; + measurements.imu.deltaT(i) = deltaT; - % create accel and gyro measurements based on - measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i); + % create accel and gyro measurements based on + measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i); - % acc = (deltaPosition - initialVel * dT) * (2/dt^2) - measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ... - - currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i))); + % acc = (deltaPosition - initialVel * dT) * (2/dt^2) + measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ... + - currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i))); - % Update velocity - currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i); + % Update velocity + currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i); - % Add Values: velocity and bias - values.insert(currentVelKey, LieVector(currentVel')); - values.insert(currentBiasKey, metadata.imu.zeroBias); -end + % Add Values: velocity and bias + values.insert(currentVelKey, LieVector(currentVel')); + values.insert(currentBiasKey, metadata.imu.zeroBias); + end end % end of IMU measurements end From b5f98622749b7ed196026e0261786b2d20cb16cd Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 16 Apr 2014 16:20:10 -0400 Subject: [PATCH 48/90] fixed bug with missing priors. Added IMU noise measurements to Monte Carlo runs --- .../+imuSimulator/covarianceAnalysisBetween.m | 76 +++++++++---------- .../covarianceAnalysisCreateFactorGraph.m | 18 +++-- .../covarianceAnalysisCreateTrajectory.m | 39 ++++++---- 3 files changed, 70 insertions(+), 63 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 4be7678c1..335ce9f6a 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -10,11 +10,11 @@ clear all close all %% Configuration -options.useRealData = 0; % controls whether or not to use the real data (if available) as the ground truth traj +options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj options.includeBetweenFactors = 1; % if true, BetweenFactors will be generated between consecutive poses options.includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the trajectory options.includeCameraFactors = 0; % not fully implemented yet -options.trajectoryLength = 4; % length of the ground truth trajectory +options.trajectoryLength = 209; % length of the ground truth trajectory options.subsampleStep = 20; numMonteCarloRuns = 2; @@ -51,8 +51,14 @@ noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4); +sigma_accel = metadata.imu.AccelerometerSigma; +sigma_gyro = metadata.imu.GyroscopeSigma; +noiseVectorAccel = [sigma_accel; sigma_accel; sigma_accel]; +noiseVectorGyro = [sigma_gyro; sigma_gyro; sigma_gyro]; + + %% Between metadata -sigma_ang = 1e-2; sigma_cart = 1; +sigma_ang = 1e-3; sigma_cart = 1e-3; noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); %noisePose = noiseModel.Isotropic.Sigma(6, 1e-3); @@ -73,10 +79,10 @@ gtNoiseModels.noisePriorPose = noisePose; gtNoiseModels.noisePriorBias = noisePriorBias; % Set measurement noise to 0, because this is ground truth -gtMeasurementNoise.poseNoiseVector = [0 0 0 0 0 0]; -gtMeasurementNoise.imu.accelNoiseVector = [0 0 0]; -gtMeasurementNoise.imu.gyroNoiseVector = [0 0 0]; -gtMeasurementNoise.cameraPixelNoiseVector = [0 0]; +gtMeasurementNoise.poseNoiseVector = [0; 0; 0; 0; 0; 0;]; +gtMeasurementNoise.imu.accelNoiseVector = [0; 0; 0]; +gtMeasurementNoise.imu.gyroNoiseVector = [0; 0; 0]; +gtMeasurementNoise.cameraPixelNoiseVector = [0; 0]; gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... gtMeasurements, ... % ground truth measurements @@ -87,8 +93,8 @@ gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... metadata); % misc data necessary for factor creation %% Display, printing, and plotting of ground truth -gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); -gtValues.print(sprintf('\nGround Truth Values:\n ')); +%gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); +%gtValues.print(sprintf('\nGround Truth Values:\n ')); warning('Additional prior on zerobias') warning('Additional PriorFactorLieVector on velocities') @@ -102,40 +108,32 @@ axis equal disp('Plotted ground truth') %% Monte Carlo Runs + +% Set up noise models +monteCarloNoiseModels.noisePose = noisePose; +monteCarloNoiseModels.noiseVel = noiseVel; +monteCarloNoiseModels.noiseBias = noiseBias; +monteCarloNoiseModels.noisePriorPose = noisePose; +monteCarloNoiseModels.noisePriorBias = noisePriorBias; + +% Set measurement noise for monte carlo runs +monteCarloMeasurementNoise.poseNoiseVector = noiseVectorPose; +monteCarloMeasurementNoise.imu.accelNoiseVector = noiseVectorAccel; +monteCarloMeasurementNoise.imu.gyroNoiseVector = noiseVectorGyro; +monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0]; + for k=1:numMonteCarloRuns fprintf('Monte Carlo Run %d.\n', k'); - - % create a new graph - graph = NonlinearFactorGraph; - - % noisy prior - currentPoseKey = symbol('x', 0); - currentPose = gtValues.at(currentPoseKey); - gtMeasurements.posePrior = currentPose; - noisyDelta = noiseVectorPose .* randn(6,1); - noisyInitialPose = Pose3.Expmap(noisyDelta); - graph.add(PriorFactorPose3(currentPoseKey, noisyInitialPose, noisePose)); - - for i=1:size(gtMeasurements.deltaMatrix,1) - currentPoseKey = symbol('x', i); - - % for each measurement: add noise and add to graph - noisyDelta = gtMeasurements.deltaMatrix(i,:)' + (noiseVectorPose .* randn(6,1)); - noisyDeltaPose = Pose3.Expmap(noisyDelta); - - % Add the factors to the factor graph - graph.add(BetweenFactorPose3(currentPoseKey-1, currentPoseKey, noisyDeltaPose, noisePose)); - end -% graph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... -% gtMeasurements, ... % ground truth measurements -% gtValues, ... % ground truth Values -% gtNoiseModels, ... % noise models to use in this graph -% gtMeasurementNoise, ... % noise to apply to measurements -% options, ... % options for the graph (e.g. which factors to include) -% metadata); % misc data necessary for factor creation + % Create a new graph using noisy measurements + graph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... + gtMeasurements, ... % ground truth measurements + gtValues, ... % ground truth Values + monteCarloNoiseModels, ... % noise models to use in this graph + monteCarloMeasurementNoise, ... % noise to apply to measurements + options, ... % options for the graph (e.g. which factors to include) + metadata); % misc data necessary for factor creation - %graph.print('graph') % optimize diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 5ca14021e..f27dbd02f 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -1,4 +1,4 @@ -function [ graph, values ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) +function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) % Create a factor graph based on provided measurements, values, and noises. % Used for covariance analysis scripts. % 'options' contains fields for including various factor types. @@ -20,19 +20,21 @@ for i=0:size(measurements.deltaMatrix, 1); %% first time step, add priors warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') warning('using identity rotation') - graph.add(PriorFactorPose3(currentPoseKey, currentPose, noiseModels.noisePose)); - measurements.posePrior = currentPose; + % Pose prior (poses used for all factors) + initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); + graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); + + % IMU velocity and bias priors if options.includeIMUFactors == 1 currentVelKey = symbol('v', 0); currentBiasKey = symbol('b', 0); currentVel = [0; 0; 0]; - values.insert(currentVelKey, LieVector(currentVel)); - values.insert(currentBiasKey, metadata.imu.zeroBias); graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, noiseModels.noisePriorBias)); end + % Camera priors if options.includeCameraFactors == 1 pointNoiseSigma = 0.1; pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); @@ -46,7 +48,7 @@ for i=0:size(measurements.deltaMatrix, 1); if options.includeBetweenFactors == 1 % Create the noisy pose estimate deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)' ... - + (measurementNoise.poseNoiseVector' .* randn(6,1))); % added noise + + (measurementNoise.poseNoiseVector .* randn(6,1))); % added noise % Add the between factor to the graph graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose)); end % end of Between pose factor creation @@ -58,9 +60,9 @@ for i=0:size(measurements.deltaMatrix, 1); currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false % Generate IMU measurements with noise imuAccel = measurements.imu.accel(i,:)' ... - + (measurementNoise.imu.accelNoiseVector' .* randn(3,1)); % added noise + + (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise imuGyro = measurements.imu.gyro(i,:)' ... - + (measurementNoise.imu.gyroNoiseVector' .* randn(3,1)); % added noise + + (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise % Initialize preintegration imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... metadata.imu.zeroBias, ... diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 6a6c3b594..d19c5cf79 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -81,30 +81,37 @@ end % end of else %% Create IMU measurements and Values for the trajectory if options.includeIMUFactors == 1 -currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements) -deltaT = 0.1; % amount of time between IMU measurements - + currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements) + deltaT = 0.1; % amount of time between IMU measurements + % Iterate over the deltaMatrix to generate appropriate IMU measurements - for i = 1:size(measurements.deltaMatrix, 1) + for i = 0:size(measurements.deltaMatrix, 1) % Update Keys currentVelKey = symbol('v', i); currentBiasKey = symbol('b', i); - measurements.imu.deltaT(i) = deltaT; + if i == 0 + % Add initial values + currentVel = [0 0 0]; + values.insert(currentVelKey, LieVector(currentVel')); + values.insert(currentBiasKey, metadata.imu.zeroBias); + else + measurements.imu.deltaT(i) = deltaT; + + % create accel and gyro measurements based on + measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i); - % create accel and gyro measurements based on - measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i); + % acc = (deltaPosition - initialVel * dT) * (2/dt^2) + measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ... + - currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i))); - % acc = (deltaPosition - initialVel * dT) * (2/dt^2) - measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ... - - currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i))); + % Update velocity + currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i); - % Update velocity - currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i); - - % Add Values: velocity and bias - values.insert(currentVelKey, LieVector(currentVel')); - values.insert(currentBiasKey, metadata.imu.zeroBias); + % Add Values: velocity and bias + values.insert(currentVelKey, LieVector(currentVel')); + values.insert(currentBiasKey, metadata.imu.zeroBias); + end end end % end of IMU measurements From e1c13c87d7e118f3bbef7fa43ec198c579c54b7a Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 17 Apr 2014 10:09:53 -0400 Subject: [PATCH 49/90] Added option for type 2 IMU factors --- .../+imuSimulator/covarianceAnalysisBetween.m | 9 +-- .../covarianceAnalysisCreateFactorGraph.m | 58 +++++++++++++------ .../covarianceAnalysisCreateTrajectory.m | 9 ++- 3 files changed, 53 insertions(+), 23 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 335ce9f6a..35b2dd52d 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -12,12 +12,13 @@ close all %% Configuration options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj options.includeBetweenFactors = 1; % if true, BetweenFactors will be generated between consecutive poses -options.includeIMUFactors = 1; % if true, IMU type 1 Factors will be generated for the trajectory +options.includeIMUFactors = 1; % if true, IMU factors will be generated for the trajectory based on options.imuFactorType +options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) options.includeCameraFactors = 0; % not fully implemented yet -options.trajectoryLength = 209; % length of the ground truth trajectory -options.subsampleStep = 20; +options.trajectoryLength = 209; % length of the ground truth trajectory +options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) -numMonteCarloRuns = 2; +numMonteCarloRuns = 2; % number of Monte Carlo runs to perform %% Camera metadata numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index f27dbd02f..ca0ce61b8 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -63,23 +63,47 @@ for i=0:size(measurements.deltaMatrix, 1); + (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise imuGyro = measurements.imu.gyro(i,:)' ... + (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise - % Initialize preintegration - imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... - metadata.imu.zeroBias, ... - metadata.imu.AccelerometerSigma.^2 * eye(3), ... - metadata.imu.GyroscopeSigma.^2 * eye(3), ... - metadata.imu.IntegrationSigma.^2 * eye(3)); - % Preintegrate - imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); - % Add Imu factor - graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... - currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); - % Add between factor on biases - graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... - noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); - % Additional prior on zerobias - graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ... - noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + + if options.imuFactorType == 2 + % Initialize preintegration + imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(... + metadata.imu.zeroBias, ... + metadata.imu.AccelerometerSigma.^2 * eye(3), ... + metadata.imu.GyroscopeSigma.^2 * eye(3), ... + metadata.imu.IntegrationSigma.^2 * eye(3), ... + metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... + metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... + metadata.imu.BiasAccOmegaInit.^2 * eye(6)); + % Preintegrate + imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); + % Add Imu factor + graph.add(CombinedImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentBiasKey, ... + imuMeasurement, ... + metadata.imu.g, metadata.imu.omegaCoriolis, ... + noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias))); + else % Assumed to be type 1 if type 2 is not selected + % Initialize preintegration + imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... + metadata.imu.zeroBias, ... + metadata.imu.AccelerometerSigma.^2 * eye(3), ... + metadata.imu.GyroscopeSigma.^2 * eye(3), ... + metadata.imu.IntegrationSigma.^2 * eye(3)); + % Preintegrate + imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); + % Add Imu factor + graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... + currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); + % Add between factor on biases + graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... + noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + % Additional prior on zerobias + graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ... + noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + end + end % end of IMU factor creation %% Add Camera factors - UNDER CONSTRUCTION !!!! - diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index d19c5cf79..d0e58afdf 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -21,13 +21,17 @@ if options.useRealData == 1 % Limit the trajectory length options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength+1]); - + fprintf('Scenario Ind: '); for i=1:options.trajectoryLength+1 % Update the pose key currentPoseKey = symbol('x', i-1); % Generate the current pose - scenarioInd = options.subsampleStep * (i-1) + 1 + scenarioInd = options.subsampleStep * (i-1) + 1; + fprintf('%d, ', scenarioInd); + if mod(i,20) == 0 + fprintf('\n'); + end gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); % truth in ENU dX = gtECEF(1) - initialPositionECEF(1); @@ -50,6 +54,7 @@ if options.useRealData == 1 measurements.deltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); end end + fprintf('\n'); else %% Create a random trajectory as ground truth currentPose = Pose3; % initial pose % initial pose From 9bc0ddd4a2b17d15c25b1fc7a48f476fe5b4abcb Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 17 Apr 2014 14:11:18 -0400 Subject: [PATCH 50/90] minor fixes --- .../+imuSimulator/covarianceAnalysisBetween.m | 37 ++++++++++--------- .../covarianceAnalysisCreateFactorGraph.m | 23 ++++++------ .../covarianceAnalysisCreateTrajectory.m | 21 ++++++----- 3 files changed, 43 insertions(+), 38 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 35b2dd52d..65ab61dad 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -1,7 +1,9 @@ import gtsam.*; -% Test GTSAM covariances on a graph with betweenFactors -% Optionally, you can also enable IMU factors and Camera factors +% Test GTSAM covariances on a factor graph with: +% Between Factors +% IMU factors +% Projection factors % Authors: Luca Carlone, David Jensen % Date: 2014/4/6 @@ -11,17 +13,20 @@ close all %% Configuration options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj -options.includeBetweenFactors = 1; % if true, BetweenFactors will be generated between consecutive poses -options.includeIMUFactors = 1; % if true, IMU factors will be generated for the trajectory based on options.imuFactorType +options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses + +options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) + options.includeCameraFactors = 0; % not fully implemented yet +numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors + options.trajectoryLength = 209; % length of the ground truth trajectory options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) numMonteCarloRuns = 2; % number of Monte Carlo runs to perform %% Camera metadata -numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration cameraMeasurementNoiseSigma = 1.0; cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); @@ -40,29 +45,27 @@ end %% Imu metadata metadata.imu.epsBias = 1e-10; % was 1e-7 metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); -metadata.imu.AccelerometerSigma = 1e-5; -metadata.imu.GyroscopeSigma = 1e-7; -metadata.imu.IntegrationSigma = 1e-4; +metadata.imu.AccelerometerSigma = 1e-3; +metadata.imu.GyroscopeSigma = 1e-5; +metadata.imu.IntegrationSigma = 1e-5; metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; -metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; +metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias; metadata.imu.g = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0]; noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 -noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); -noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-4); +noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); % between on biases +noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-6); sigma_accel = metadata.imu.AccelerometerSigma; sigma_gyro = metadata.imu.GyroscopeSigma; -noiseVectorAccel = [sigma_accel; sigma_accel; sigma_accel]; -noiseVectorGyro = [sigma_gyro; sigma_gyro; sigma_gyro]; - +noiseVectorAccel = sigma_accel * ones(3,1); +noiseVectorGyro = sigma_gyro * ones(3,1); %% Between metadata -sigma_ang = 1e-3; sigma_cart = 1e-3; -noiseVectorPose = [sigma_ang; sigma_ang; sigma_ang; sigma_cart; sigma_cart; sigma_cart]; +sigma_ang = 1e-2; sigma_cart = 1e-1; +noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); -%noisePose = noiseModel.Isotropic.Sigma(6, 1e-3); %% Set log files testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index ca0ce61b8..70a9a5bda 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -17,10 +17,7 @@ for i=0:size(measurements.deltaMatrix, 1); currentPose = values.at(currentPoseKey); if i==0 - %% first time step, add priors - warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') - warning('using identity rotation') - + %% first time step, add priors % Pose prior (poses used for all factors) initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); @@ -28,10 +25,12 @@ for i=0:size(measurements.deltaMatrix, 1); % IMU velocity and bias priors if options.includeIMUFactors == 1 currentVelKey = symbol('v', 0); - currentBiasKey = symbol('b', 0); - currentVel = [0; 0; 0]; + currentVel = values.at(currentVelKey).vector; graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); - graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, noiseModels.noisePriorBias)); + + currentBiasKey = symbol('b', 0); + currentBias = values.at(currentBiasKey); + graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias)); end % Camera priors @@ -84,7 +83,7 @@ for i=0:size(measurements.deltaMatrix, 1); imuMeasurement, ... metadata.imu.g, metadata.imu.omegaCoriolis, ... noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias))); - else % Assumed to be type 1 if type 2 is not selected + else % IMU type 1 % Initialize preintegration imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... metadata.imu.zeroBias, ... @@ -100,8 +99,8 @@ for i=0:size(measurements.deltaMatrix, 1); graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); % Additional prior on zerobias - graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ... - noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + %graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ... + % noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); end end % end of IMU factor creation @@ -128,9 +127,9 @@ for i=0:size(measurements.deltaMatrix, 1); %fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped); end % end of Camera factor creation - end % end of else + end % end of else (i=0) end % end of for over trajectory -end +end % end of function diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index d0e58afdf..9c54af687 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -1,4 +1,4 @@ -function [ values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata ) +function [values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata ) % Create a trajectory for running covariance analysis scripts. % 'options' contains fields for including various factor types and setting trajectory length % 'metadata' is a storage variable for miscellaneous factor-specific values @@ -9,6 +9,9 @@ import gtsam.*; values = Values; + warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') + warning('using identity rotation') + if options.useRealData == 1 %% Create a ground truth trajectory from Real data (if available) fprintf('\nUsing real data as ground truth\n'); @@ -26,7 +29,7 @@ if options.useRealData == 1 % Update the pose key currentPoseKey = symbol('x', i-1); - % Generate the current pose + % Generate the current pose scenarioInd = options.subsampleStep * (i-1) + 1; fprintf('%d, ', scenarioInd); if mod(i,20) == 0 @@ -40,7 +43,7 @@ if options.useRealData == 1 [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); gtPosition = [xlt, ylt, zlt]'; - gtRotation = Rot3; % Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); + gtRotation = Rot3; %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); currentPose = Pose3(gtRotation, Point3(gtPosition)); % Add values @@ -94,7 +97,7 @@ if options.includeIMUFactors == 1 % Update Keys currentVelKey = symbol('v', i); currentBiasKey = symbol('b', i); - + if i == 0 % Add initial values currentVel = [0 0 0]; @@ -105,14 +108,14 @@ if options.includeIMUFactors == 1 % create accel and gyro measurements based on measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i); - + % acc = (deltaPosition - initialVel * dT) * (2/dt^2) measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ... - - currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i))); - + - currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i))); + % Update velocity currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i); - + % Add Values: velocity and bias values.insert(currentVelKey, LieVector(currentVel')); values.insert(currentBiasKey, metadata.imu.zeroBias); @@ -120,5 +123,5 @@ if options.includeIMUFactors == 1 end end % end of IMU measurements -end +end % end of function From 2e3dcd2ab71b34c9e7186612df731b7f8855fdc7 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 17 Apr 2014 15:23:01 -0400 Subject: [PATCH 51/90] code reorganization --- .../+imuSimulator/covarianceAnalysisBetween.m | 11 +- .../covarianceAnalysisCreateFactorGraph.m | 103 +++++++-------- .../covarianceAnalysisCreateTrajectory.m | 120 ++++++++---------- .../+imuSimulator/getPoseFromGtScenario.m | 25 ++++ 4 files changed, 135 insertions(+), 124 deletions(-) create mode 100644 matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 65ab61dad..1de2b1f44 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -100,9 +100,6 @@ gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... %gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); %gtValues.print(sprintf('\nGround Truth Values:\n ')); -warning('Additional prior on zerobias') -warning('Additional PriorFactorLieVector on velocities') - figure(1) hold on; plot3DPoints(gtValues); @@ -150,9 +147,9 @@ for k=1:numMonteCarloRuns marginals = Marginals(graph, estimate); % for each pose in the trajectory - for i=1:size(gtMeasurements.deltaMatrix,1)+1 + for i=0:options.trajectoryLength % compute estimation errors - currentPoseKey = symbol('x', i-1); + currentPoseKey = symbol('x', i); gtPosition = gtValues.at(currentPoseKey).translation.vector; estPosition = estimate.at(currentPoseKey).translation.vector; estR = estimate.at(currentPoseKey).rotation.matrix; @@ -163,7 +160,7 @@ for k=1:numMonteCarloRuns covPosition = estR * cov(4:6,4:6) * estR'; % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances - NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof + NEES(k,i+1) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof end figure(2) @@ -215,7 +212,7 @@ title('NEES normalized by dof VS bounds'); saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig'); logFile = horzcat(folderName,'log-',testName); -save(logFile) +%save(logFile) %% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4) % the nees for a single experiment (i) is defined as diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 70a9a5bda..ee6835367 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -1,4 +1,4 @@ -function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) +function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) % Create a factor graph based on provided measurements, values, and noises. % Used for covariance analysis scripts. % 'options' contains fields for including various factor types. @@ -10,14 +10,14 @@ import gtsam.*; graph = NonlinearFactorGraph; -% Iterate through the trajectory -for i=0:size(measurements.deltaMatrix, 1); +% Iterate through the trajectory +for i=0:length(measurements) % Get the current pose currentPoseKey = symbol('x', i); - currentPose = values.at(currentPoseKey); + currentPose = values.at(currentPoseKey); if i==0 - %% first time step, add priors + %% first time step, add priors % Pose prior (poses used for all factors) initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); @@ -25,11 +25,11 @@ for i=0:size(measurements.deltaMatrix, 1); % IMU velocity and bias priors if options.includeIMUFactors == 1 currentVelKey = symbol('v', 0); - currentVel = values.at(currentVelKey).vector; + currentVel = values.at(currentVelKey).vector; graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); currentBiasKey = symbol('b', 0); - currentBias = values.at(currentBiasKey); + currentBias = values.at(currentBiasKey); graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias)); end @@ -41,12 +41,12 @@ for i=0:size(measurements.deltaMatrix, 1); end else - prevPoseKey = symbol('x', i-1); - + %% Add Between factors if options.includeBetweenFactors == 1 + prevPoseKey = symbol('x', i-1); % Create the noisy pose estimate - deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)' ... + deltaPose = Pose3.Expmap(measurements(i).deltaVector ... + (measurementNoise.poseNoiseVector .* randn(6,1))); % added noise % Add the between factor to the graph graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose)); @@ -57,52 +57,55 @@ for i=0:size(measurements.deltaMatrix, 1); % Update keys currentVelKey = symbol('v', i); % not used if includeIMUFactors is false currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false - % Generate IMU measurements with noise - imuAccel = measurements.imu.accel(i,:)' ... - + (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise - imuGyro = measurements.imu.gyro(i,:)' ... - + (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise - - if options.imuFactorType == 2 - % Initialize preintegration - imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(... - metadata.imu.zeroBias, ... - metadata.imu.AccelerometerSigma.^2 * eye(3), ... - metadata.imu.GyroscopeSigma.^2 * eye(3), ... - metadata.imu.IntegrationSigma.^2 * eye(3), ... - metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... - metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... - metadata.imu.BiasAccOmegaInit.^2 * eye(6)); - % Preintegrate - imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); - % Add Imu factor - graph.add(CombinedImuFactor( ... - currentPoseKey-1, currentVelKey-1, ... - currentPoseKey, currentVelKey, ... - currentBiasKey-1, currentBiasKey, ... - imuMeasurement, ... - metadata.imu.g, metadata.imu.omegaCoriolis, ... - noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias))); - else % IMU type 1 + + if options.imuFactorType == 1 % Initialize preintegration imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... metadata.imu.zeroBias, ... metadata.imu.AccelerometerSigma.^2 * eye(3), ... metadata.imu.GyroscopeSigma.^2 * eye(3), ... metadata.imu.IntegrationSigma.^2 * eye(3)); - % Preintegrate - imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); - % Add Imu factor - graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... - currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); - % Add between factor on biases - graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... - noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); - % Additional prior on zerobias - %graph.add(PriorFactorConstantBias(currentBiasKey, metadata.imu.zeroBias, ... - % noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + % Generate IMU measurements with noise + for j=1:length(measurements(i).imu) % all measurements to preintegrate + imuAccel = measurements(i).imu(j).accel ... + + (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise + imuGyro = measurements(i).imu(j).gyro ... + + (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise + + % Preintegrate + imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT); + % Add Imu factor + graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... + currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); + % Add between factor on biases + graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... + noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + end end - + + + if options.imuFactorType == 2 + % % Initialize preintegration + % imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(... + % metadata.imu.zeroBias, ... + % metadata.imu.AccelerometerSigma.^2 * eye(3), ... + % metadata.imu.GyroscopeSigma.^2 * eye(3), ... + % metadata.imu.IntegrationSigma.^2 * eye(3), ... + % metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... + % metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... + % metadata.imu.BiasAccOmegaInit.^2 * eye(6)); + % % Preintegrate + % imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); + % % Add Imu factor + % graph.add(CombinedImuFactor( ... + % currentPoseKey-1, currentVelKey-1, ... + % currentPoseKey, currentVelKey, ... + % currentBiasKey-1, currentBiasKey, ... + % imuMeasurement, ... + % metadata.imu.g, metadata.imu.omegaCoriolis, ... + % noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias))); + end + end % end of IMU factor creation %% Add Camera factors - UNDER CONSTRUCTION !!!! - @@ -115,7 +118,7 @@ for i=0:size(measurements.deltaMatrix, 1); landmarkKey = symbol('p', j); try Z = gtCamera.project(gtLandmarkPoints(j)); - %% TO-DO probably want to do some type of filtering on the measurement values, because + % TO-DO probably want to do some type of filtering on the measurement values, because % they might not all be valid graph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K)); catch diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 9c54af687..8712c818c 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -9,8 +9,8 @@ import gtsam.*; values = Values; - warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') - warning('using identity rotation') +warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') +warning('using identity rotation') if options.useRealData == 1 %% Create a ground truth trajectory from Real data (if available) @@ -18,47 +18,69 @@ if options.useRealData == 1 gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',... 'VEast', 'VNorth', 'VUp'); - Org_lat = gtScenario.Lat(1); - Org_lon = gtScenario.Lon(1); - initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]); - % Limit the trajectory length - options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength+1]); + options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength]); fprintf('Scenario Ind: '); - for i=1:options.trajectoryLength+1 - % Update the pose key - currentPoseKey = symbol('x', i-1); - - % Generate the current pose - scenarioInd = options.subsampleStep * (i-1) + 1; + + for i=0:options.trajectoryLength + scenarioInd = options.subsampleStep * i + 1; fprintf('%d, ', scenarioInd); - if mod(i,20) == 0 - fprintf('\n'); - end - gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); - % truth in ENU - dX = gtECEF(1) - initialPositionECEF(1); - dY = gtECEF(2) - initialPositionECEF(2); - dZ = gtECEF(3) - initialPositionECEF(3); - [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); + if (mod(i,20) == 0) fprintf('\n'); end - gtPosition = [xlt, ylt, zlt]'; - gtRotation = Rot3; %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); - currentPose = Pose3(gtRotation, Point3(gtPosition)); - - % Add values + %% Generate the current pose + currentPoseKey = symbol('x', i); + currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd); + % add to values values.insert(currentPoseKey, currentPose); - % Generate the measurement. The first pose is considered the prior, so - % it has no measurement - if i > 1 + %% gt Between measurements + if options.includeBetweenFactors == 1 && i > 0 prevPose = values.at(currentPoseKey - 1); deltaPose = prevPose.between(currentPose); - measurements.deltaMatrix(i-1,:) = Pose3.Logmap(deltaPose); + measurements(i).deltaVector = Pose3.Logmap(deltaPose); end + + %% gt IMU measurements + if options.includeIMUFactors == 1 + currentVelKey = symbol('v', i); + currentBiasKey = symbol('b', i); + deltaT = 0.01; % amount of time between IMU measurements + if i == 0 + currentVel = [0 0 0]'; + else + % integrate & store intermediate measurements + for j=1:options.subsampleStep % we integrate all the intermediate measurements + scenarioIndIMU1 = scenarioInd+j-1; + scenarioIndIMU2 = scenarioInd+j; + IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1); + IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2); + IMUdeltaPose = IMUPose1.between(IMUPose2); + IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose); + IMUdeltaRotVector = IMUdeltaPoseVector(1:3); + IMUdeltaPositionVector = IMUPose2.translation.vector - IMUPose1.translation.vector; % translation in absolute frame + + measurements(i).imu(j).deltaT = deltaT; + + % gyro rate: Logmap(R_i' * R_i+1) / deltaT + measurements(i).imu(j).gyro = IMUdeltaRotVector./deltaT; + + % acc = (deltaPosition - initialVel * dT) * (2/dt^2) + measurements(i).imu(j).accel = (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT)); + + % Update velocity + currentVel = IMUdeltaPositionVector./deltaT; + end + end + + % Add Values: velocity and bias + values.insert(currentVelKey, LieVector(currentVel)); + values.insert(currentBiasKey, metadata.imu.zeroBias); + end + end fprintf('\n'); else + error('Please use RealData') %% Create a random trajectory as ground truth currentPose = Pose3; % initial pose % initial pose @@ -87,41 +109,5 @@ else end % end of random trajectory creation end % end of else -%% Create IMU measurements and Values for the trajectory -if options.includeIMUFactors == 1 - currentVel = [0 0 0]; % initial velocity (used to generate IMU measurements) - deltaT = 0.1; % amount of time between IMU measurements - - % Iterate over the deltaMatrix to generate appropriate IMU measurements - for i = 0:size(measurements.deltaMatrix, 1) - % Update Keys - currentVelKey = symbol('v', i); - currentBiasKey = symbol('b', i); - - if i == 0 - % Add initial values - currentVel = [0 0 0]; - values.insert(currentVelKey, LieVector(currentVel')); - values.insert(currentBiasKey, metadata.imu.zeroBias); - else - measurements.imu.deltaT(i) = deltaT; - - % create accel and gyro measurements based on - measurements.imu.gyro(i,:) = measurements.deltaMatrix(i, 1:3)./measurements.imu.deltaT(i); - - % acc = (deltaPosition - initialVel * dT) * (2/dt^2) - measurements.imu.accel(i,:) = (measurements.deltaMatrix(i, 4:6) ... - - currentVel.*measurements.imu.deltaT(i)).*(2/(measurements.imu.deltaT(i)*measurements.imu.deltaT(i))); - - % Update velocity - currentVel = measurements.deltaMatrix(i,4:6)./measurements.imu.deltaT(i); - - % Add Values: velocity and bias - values.insert(currentVelKey, LieVector(currentVel')); - values.insert(currentBiasKey, metadata.imu.zeroBias); - end - end -end % end of IMU measurements - end % end of function diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m new file mode 100644 index 000000000..61830443a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m @@ -0,0 +1,25 @@ +function currentPose = getPoseFromGtScenario(gtScenario,scenarioInd) +% gtScenario contains vectors (Lat, Lon, Alt, Roll, Pitch, Yaw) +% The function picks the index 'scenarioInd' in those vectors and +% computes the corresponding pose by +% 1) Converting (Lat,Lon,Alt) to local coordinates +% 2) Converting (Roll,Pitch,Yaw) to a rotation matrix + +import gtsam.*; + +Org_lat = gtScenario.Lat(1); +Org_lon = gtScenario.Lon(1); +initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]); + +gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); +% truth in ENU +dX = gtECEF(1) - initialPositionECEF(1); +dY = gtECEF(2) - initialPositionECEF(2); +dZ = gtECEF(3) - initialPositionECEF(3); +[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); + +gtPosition = [xlt, ylt, zlt]'; +gtRotation = Rot3; %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); +currentPose = Pose3(gtRotation, Point3(gtPosition)); + +end \ No newline at end of file From 3cae615991c83a448a273b991006ad701ccbb863 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 17 Apr 2014 16:00:18 -0400 Subject: [PATCH 52/90] amost fixed imu simulation --- .../+imuSimulator/covarianceAnalysisBetween.m | 29 +++++++++++++------ .../covarianceAnalysisCreateFactorGraph.m | 14 +++++---- .../covarianceAnalysisCreateTrajectory.m | 3 +- .../+imuSimulator/getPoseFromGtScenario.m | 2 +- 4 files changed, 31 insertions(+), 17 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 1de2b1f44..6162d8309 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -11,6 +11,8 @@ clc clear all close all +saveResults = 0; + %% Configuration options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses @@ -106,6 +108,13 @@ plot3DPoints(gtValues); plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); axis equal +% optimize +optimizer = GaussNewtonOptimizer(gtGraph, gtValues); +gtEstimate = optimizer.optimize(); +% estimate should match gtValues if graph is correct. +fprintf('Error in ground truth graph at gtValues: %g \n', gtGraph.error(gtValues) ); +fprintf('Error in ground truth graph at gtEstimate: %g \n', gtGraph.error(gtEstimate) ); + disp('Plotted ground truth') %% Monte Carlo Runs @@ -174,16 +183,18 @@ plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof box on set(gca,'Fontsize',16) title('NEES and ANEES'); -%print('-djpeg', horzcat('runs-',testName)); -saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig'); +if saveResults + saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig'); +end %% figure(1) box on set(gca,'Fontsize',16) title('Ground truth and estimates for each MC runs'); -%print('-djpeg', horzcat('gt-',testName)); -saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig'); +if saveResults + saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig'); +end %% Let us compute statistics on the overall NEES n = 3; % position vector dimension @@ -208,11 +219,11 @@ plot(r2*ones(size(ANEES,2),1),'k-.'); box on set(gca,'Fontsize',16) title('NEES normalized by dof VS bounds'); -%print('-djpeg', horzcat('ANEES-',testName)); -saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig'); - -logFile = horzcat(folderName,'log-',testName); -%save(logFile) +if saveResults + saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig'); + logFile = horzcat(folderName,'log-',testName); + save(logFile) +end %% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4) % the nees for a single experiment (i) is defined as diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index ee6835367..cb10d14cd 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -74,13 +74,15 @@ for i=0:length(measurements) % Preintegrate imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT); - % Add Imu factor - graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... - currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); - % Add between factor on biases - graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... - noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); end + %imuMeasurement.print('imuMeasurement'); + + % Add Imu factor + graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... + currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); + % Add between factor on biases + graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... + noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); end diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 8712c818c..21c263301 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -64,11 +64,12 @@ if options.useRealData == 1 % gyro rate: Logmap(R_i' * R_i+1) / deltaT measurements(i).imu(j).gyro = IMUdeltaRotVector./deltaT; + % deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; % acc = (deltaPosition - initialVel * dT) * (2/dt^2) measurements(i).imu(j).accel = (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT)); % Update velocity - currentVel = IMUdeltaPositionVector./deltaT; + currentVel = currentVel + measurements(i).imu(j).accel * deltaT; end end diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m index 61830443a..73c60efc3 100644 --- a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m +++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m @@ -19,7 +19,7 @@ dZ = gtECEF(3) - initialPositionECEF(3); [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); gtPosition = [xlt, ylt, zlt]'; -gtRotation = Rot3; %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); +gtRotation = Rot3; %Rot3.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); currentPose = Pose3(gtRotation, Point3(gtPosition)); end \ No newline at end of file From 322e3e08c8fb3c9baaf494685633e1e7bfd60d97 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 17 Apr 2014 16:08:38 -0400 Subject: [PATCH 53/90] consistent IMU factor1 --- .../covarianceAnalysisCreateTrajectory.m | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 21c263301..105fe3e80 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -48,12 +48,18 @@ if options.useRealData == 1 if i == 0 currentVel = [0 0 0]'; else - % integrate & store intermediate measurements + % integrate & store intermediate measurements for j=1:options.subsampleStep % we integrate all the intermediate measurements - scenarioIndIMU1 = scenarioInd+j-1; - scenarioIndIMU2 = scenarioInd+j; + previousScenarioInd = options.subsampleStep * (i-1) + 1; + scenarioIndIMU1 = previousScenarioInd+j-1; + scenarioIndIMU2 = previousScenarioInd+j; IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1); IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2); + +% scenarioIndIMU1 +% IMUPose1.print('IMUPose1'); +% IMUPose2.print('IMUPose2'); + IMUdeltaPose = IMUPose1.between(IMUPose2); IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose); IMUdeltaRotVector = IMUdeltaPoseVector(1:3); From a0a955e5a5d4967802474a68f65a7313822cf5d6 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 17 Apr 2014 16:14:32 -0400 Subject: [PATCH 54/90] fixing imu simulator for non-identical rotations --- .../covarianceAnalysisCreateFactorGraph.m | 3 +-- .../covarianceAnalysisCreateTrajectory.m | 14 +++++--------- .../+imuSimulator/getPoseFromGtScenario.m | 2 +- 3 files changed, 7 insertions(+), 12 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index cb10d14cd..d19619474 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -84,8 +84,7 @@ for i=0:length(measurements) graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); end - - + if options.imuFactorType == 2 % % Initialize preintegration % imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(... diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 105fe3e80..5f225f4df 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -9,8 +9,7 @@ import gtsam.*; values = Values; -warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') -warning('using identity rotation') +warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data - using identity rotation') if options.useRealData == 1 %% Create a ground truth trajectory from Real data (if available) @@ -55,11 +54,8 @@ if options.useRealData == 1 scenarioIndIMU2 = previousScenarioInd+j; IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1); IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2); - -% scenarioIndIMU1 -% IMUPose1.print('IMUPose1'); -% IMUPose2.print('IMUPose2'); - + IMURot2 = IMUPose2.rotation.matrix; + IMUdeltaPose = IMUPose1.between(IMUPose2); IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose); IMUdeltaRotVector = IMUdeltaPoseVector(1:3); @@ -72,10 +68,10 @@ if options.useRealData == 1 % deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; % acc = (deltaPosition - initialVel * dT) * (2/dt^2) - measurements(i).imu(j).accel = (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT)); + measurements(i).imu(j).accel = IMURot2' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT)); % Update velocity - currentVel = currentVel + measurements(i).imu(j).accel * deltaT; + currentVel = currentVel + IMURot2 * measurements(i).imu(j).accel * deltaT; end end diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m index 73c60efc3..bf8da3343 100644 --- a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m +++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m @@ -19,7 +19,7 @@ dZ = gtECEF(3) - initialPositionECEF(3); [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); gtPosition = [xlt, ylt, zlt]'; -gtRotation = Rot3; %Rot3.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); +gtRotation = Rot3; % Rot3.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); currentPose = Pose3(gtRotation, Point3(gtPosition)); end \ No newline at end of file From 26c296603fba3211eb5dbf6d6c75ad0e8550dab2 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 17 Apr 2014 16:23:29 -0400 Subject: [PATCH 55/90] fixed use of 2nd order integration in matlab wrapper --- gtsam.h | 3 ++- .../+imuSimulator/covarianceAnalysisCreateFactorGraph.m | 4 +++- .../+imuSimulator/covarianceAnalysisCreateTrajectory.m | 2 +- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam.h b/gtsam.h index de4c1381d..5d86110aa 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2286,7 +2286,8 @@ virtual class ConstantBias : gtsam::Value { #include class ImuFactorPreintegratedMeasurements { // Standard Constructor - ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); + ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); + ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); // Testable diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index d19619474..33bce851c 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -59,12 +59,14 @@ for i=0:length(measurements) currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false if options.imuFactorType == 1 + use2ndOrderIntegration = true; % Initialize preintegration imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... metadata.imu.zeroBias, ... metadata.imu.AccelerometerSigma.^2 * eye(3), ... metadata.imu.GyroscopeSigma.^2 * eye(3), ... - metadata.imu.IntegrationSigma.^2 * eye(3)); + metadata.imu.IntegrationSigma.^2 * eye(3), ... + use2ndOrderIntegration); % Generate IMU measurements with noise for j=1:length(measurements(i).imu) % all measurements to preintegrate imuAccel = measurements(i).imu(j).accel ... diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 5f225f4df..cce0f69ee 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -9,7 +9,7 @@ import gtsam.*; values = Values; -warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data - using identity rotation') +warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data - currently using identity rotation') if options.useRealData == 1 %% Create a ground truth trajectory from Real data (if available) From 5abf0b01ea61d038c19ce287e95083c72f3f0a16 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 17 Apr 2014 22:21:22 -0400 Subject: [PATCH 56/90] Added option for constant IMU bias --- .../+imuSimulator/covarianceAnalysisBetween.m | 27 ++++++++++++++----- .../covarianceAnalysisCreateFactorGraph.m | 6 +++-- 2 files changed, 25 insertions(+), 8 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 6162d8309..5768a068b 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -19,6 +19,7 @@ options.includeBetweenFactors = 1; % if true, BetweenFactors will be added betwe options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) +options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements options.includeCameraFactors = 0; % not fully implemented yet numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors @@ -57,7 +58,8 @@ metadata.imu.g = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0]; noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); % between on biases -noisePriorBias = noiseModel.Isotropic.Sigma(6, 1e-6); +noisePriorBias = noiseModel.Diagonal.Sigmas([metadata.imu.BiasAccelerometerSigma * ones(3,1); ... + metadata.imu.BiasGyroscopeSigma * ones(3,1)]); sigma_accel = metadata.imu.AccelerometerSigma; sigma_gyro = metadata.imu.GyroscopeSigma; @@ -70,7 +72,7 @@ noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); %% Set log files -testName = sprintf('sa-%1.2g-sc-%1.2g',sigma_ang,sigma_cart) +testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro) folderName = 'results/' %% Create ground truth trajectory and measurements @@ -85,11 +87,15 @@ gtNoiseModels.noisePriorPose = noisePose; gtNoiseModels.noisePriorBias = noisePriorBias; % Set measurement noise to 0, because this is ground truth -gtMeasurementNoise.poseNoiseVector = [0; 0; 0; 0; 0; 0;]; -gtMeasurementNoise.imu.accelNoiseVector = [0; 0; 0]; -gtMeasurementNoise.imu.gyroNoiseVector = [0; 0; 0]; -gtMeasurementNoise.cameraPixelNoiseVector = [0; 0]; +gtMeasurementNoise.poseNoiseVector = zeros(6,1); +gtMeasurementNoise.imu.accelNoiseVector = zeros(3,1); +gtMeasurementNoise.imu.gyroNoiseVector = zeros(3,1); +gtMeasurementNoise.cameraPixelNoiseVector = zeros(2,1); +% Set IMU biases to zero +metadata.imu.accelConstantBiasVector = zeros(3,1); +metadata.imu.gyroConstantBiasVector = zeros(3,1); + gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... gtMeasurements, ... % ground truth measurements gtValues, ... % ground truth Values @@ -135,6 +141,15 @@ monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0]; for k=1:numMonteCarloRuns fprintf('Monte Carlo Run %d.\n', k'); + % Create a random bias for each run + if options.imuNonzeroBias == 1 + metadata.imu.accelBiasVector = metadata.imu.BiasAccelerometerSigma .* randn(3,1); + metadata.imu.gyroBiasVector = metadata.imu.BiasGyroscopeSigma .* randn(3,1); + else + metadata.imu.accelConstantBiasVector = zeros(3,1); + metadata.imu.gyroConstantBiasVector = zeros(3,1); + end + % Create a new graph using noisy measurements graph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... gtMeasurements, ... % ground truth measurements diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 33bce851c..48afa7ed6 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -70,9 +70,11 @@ for i=0:length(measurements) % Generate IMU measurements with noise for j=1:length(measurements(i).imu) % all measurements to preintegrate imuAccel = measurements(i).imu(j).accel ... - + (measurementNoise.imu.accelNoiseVector .* randn(3,1)); % added noise + + (measurementNoise.imu.accelNoiseVector .* randn(3,1))... % added noise + + metadata.imu.accelConstantBiasVector; % constant bias imuGyro = measurements(i).imu(j).gyro ... - + (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); % added noise + + (measurementNoise.imu.gyroNoiseVector .* randn(3,1))... % added noise + + metadata.imu.gyroConstantBiasVector; % constant bias % Preintegrate imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT); From 13d47fcee4235cf0858fc41092194eacc4247646 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 23 Apr 2014 08:58:50 -0400 Subject: [PATCH 57/90] added IMU type 2 with noise --- .../covarianceAnalysisCreateFactorGraph.m | 48 +++++++++++-------- 1 file changed, 29 insertions(+), 19 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 48afa7ed6..094dd10b2 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -90,25 +90,35 @@ for i=0:length(measurements) end if options.imuFactorType == 2 - % % Initialize preintegration - % imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(... - % metadata.imu.zeroBias, ... - % metadata.imu.AccelerometerSigma.^2 * eye(3), ... - % metadata.imu.GyroscopeSigma.^2 * eye(3), ... - % metadata.imu.IntegrationSigma.^2 * eye(3), ... - % metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... - % metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... - % metadata.imu.BiasAccOmegaInit.^2 * eye(6)); - % % Preintegrate - % imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i)); - % % Add Imu factor - % graph.add(CombinedImuFactor( ... - % currentPoseKey-1, currentVelKey-1, ... - % currentPoseKey, currentVelKey, ... - % currentBiasKey-1, currentBiasKey, ... - % imuMeasurement, ... - % metadata.imu.g, metadata.imu.omegaCoriolis, ... - % noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias))); + % Initialize preintegration + imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(... + metadata.imu.zeroBias, ... + metadata.imu.AccelerometerSigma.^2 * eye(3), ... + metadata.imu.GyroscopeSigma.^2 * eye(3), ... + metadata.imu.IntegrationSigma.^2 * eye(3), ... + metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... + metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... + metadata.imu.BiasAccOmegaInit.^2 * eye(6)); + % Generate IMU measurements with noise + for j=1:length(measurements(i).imu) % all measurements to preintegrate + imuAccel = measurements(i).imu(j).accel ... + + (measurementNoise.imu.accelNoiseVector .* randn(3,1))... % added noise + + metadata.imu.accelConstantBiasVector; % constant bias + imuGyro = measurements(i).imu(j).gyro ... + + (measurementNoise.imu.gyroNoiseVector .* randn(3,1))... % added noise + + metadata.imu.gyroConstantBiasVector; % constant bias + + % Preintegrate + imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT); + end + % Add Imu factor + graph.add(CombinedImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentBiasKey, ... + imuMeasurement, ... + metadata.imu.g, metadata.imu.omegaCoriolis, ... + noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias))); end end % end of IMU factor creation From 2ab81ae9973c37556e96012587c3085f9b2b1f5d Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 23 Apr 2014 12:39:47 -0400 Subject: [PATCH 58/90] working on GPS factors --- .../+imuSimulator/covarianceAnalysisBetween.m | 4 +++- .../+imuSimulator/covarianceAnalysisCreateFactorGraph.m | 7 +++++++ .../+imuSimulator/covarianceAnalysisCreateTrajectory.m | 8 +++++++- 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 5768a068b..3b2486160 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -21,9 +21,11 @@ options.includeIMUFactors = 1; % if true, IMU factors will be added between options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements -options.includeCameraFactors = 0; % not fully implemented yet +options.includeCameraFactors = 0; % not fully implementemeasurements, values, noiseModels, measurementNoise, options, metadatad yet numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors +options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses + options.trajectoryLength = 209; % length of the ground truth trajectory options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 094dd10b2..396a5a4eb 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -145,6 +145,13 @@ for i=0:length(measurements) %fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped); end % end of Camera factor creation + %% Add GPS factors + if options.includeGPSFactors == 1 + currentGPSKey = symbol('g', i); + graph.add(GPSPriorFactor(currentPoseKey, measurements(i).gpsPosition, ... + noiseModel.Isotropic.Sigma(3, 1e-4))); + end % end of GPS factor creation + end % end of else (i=0) end % end of for over trajectory diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index cce0f69ee..e895fb8a6 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -24,7 +24,7 @@ if options.useRealData == 1 for i=0:options.trajectoryLength scenarioInd = options.subsampleStep * i + 1; fprintf('%d, ', scenarioInd); - if (mod(i,20) == 0) fprintf('\n'); end + if (mod(i,12) == 0) fprintf('\n'); end %% Generate the current pose currentPoseKey = symbol('x', i); @@ -80,6 +80,12 @@ if options.useRealData == 1 values.insert(currentBiasKey, metadata.imu.zeroBias); end + %% gt GPS measurements + if options.includeGPSFactors == 1 && i > 0 + gpsPosition = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector; + measurements(i).gpsPosition = Point3(gpsPosition); + end + end fprintf('\n'); else From 87f9e5bb2c483bceb6ca1a446d5c694483618205 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 23 Apr 2014 14:45:17 -0400 Subject: [PATCH 59/90] completed GPS factors --- .../+imuSimulator/covarianceAnalysisBetween.m | 13 +++++++++++-- .../covarianceAnalysisCreateFactorGraph.m | 8 +++++--- .../covarianceAnalysisCreateTrajectory.m | 4 ++-- 3 files changed, 18 insertions(+), 7 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 3b2486160..1419f1773 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -15,7 +15,7 @@ saveResults = 0; %% Configuration options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj -options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses +options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) @@ -29,7 +29,7 @@ options.includeGPSFactors = 0; % if true, GPS factors will be added as prior options.trajectoryLength = 209; % length of the ground truth trajectory options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) -numMonteCarloRuns = 2; % number of Monte Carlo runs to perform +numMonteCarloRuns = 50; % number of Monte Carlo runs to perform %% Camera metadata K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration @@ -73,6 +73,11 @@ sigma_ang = 1e-2; sigma_cart = 1e-1; noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); +%% GPS metadata +sigma_gps = 1e-4; +noiseVectorGPS = sigma_gps * ones(3,1); +noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]); + %% Set log files testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro) folderName = 'results/' @@ -87,12 +92,14 @@ gtNoiseModels.noiseVel = noiseVel; gtNoiseModels.noiseBias = noiseBias; gtNoiseModels.noisePriorPose = noisePose; gtNoiseModels.noisePriorBias = noisePriorBias; +gtNoiseModels.noiseGPS = noiseGPS; % Set measurement noise to 0, because this is ground truth gtMeasurementNoise.poseNoiseVector = zeros(6,1); gtMeasurementNoise.imu.accelNoiseVector = zeros(3,1); gtMeasurementNoise.imu.gyroNoiseVector = zeros(3,1); gtMeasurementNoise.cameraPixelNoiseVector = zeros(2,1); +gtMeasurementNoise.gpsNoiseVector = zeros(3,1); % Set IMU biases to zero metadata.imu.accelConstantBiasVector = zeros(3,1); @@ -133,12 +140,14 @@ monteCarloNoiseModels.noiseVel = noiseVel; monteCarloNoiseModels.noiseBias = noiseBias; monteCarloNoiseModels.noisePriorPose = noisePose; monteCarloNoiseModels.noisePriorBias = noisePriorBias; +monteCarloNoiseModels.noiseGPS = noiseGPS; % Set measurement noise for monte carlo runs monteCarloMeasurementNoise.poseNoiseVector = noiseVectorPose; monteCarloMeasurementNoise.imu.accelNoiseVector = noiseVectorAccel; monteCarloMeasurementNoise.imu.gyroNoiseVector = noiseVectorGyro; monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0]; +monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS; for k=1:numMonteCarloRuns fprintf('Monte Carlo Run %d.\n', k'); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 396a5a4eb..921d42679 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -147,9 +147,11 @@ for i=0:length(measurements) %% Add GPS factors if options.includeGPSFactors == 1 - currentGPSKey = symbol('g', i); - graph.add(GPSPriorFactor(currentPoseKey, measurements(i).gpsPosition, ... - noiseModel.Isotropic.Sigma(3, 1e-4))); + gpsPosition = measurements(i).gpsPositionVector ... + + measurementNoise.gpsNoiseVector .* randn(3,1); + graph.add(PriorFactorPose3(currentPoseKey, ... + Pose3(currentPose.rotation, Point3(gpsPosition)), ... + noiseModels.noiseGPS)); end % end of GPS factor creation end % end of else (i=0) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index e895fb8a6..3933870b5 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -82,8 +82,8 @@ if options.useRealData == 1 %% gt GPS measurements if options.includeGPSFactors == 1 && i > 0 - gpsPosition = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector; - measurements(i).gpsPosition = Point3(gpsPosition); + gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector; + measurements(i).gpsPositionVector = gpsPositionVector; end end From c1ab0053eb987d466b4fc9454d4515115120d6a0 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 23 Apr 2014 16:06:03 -0400 Subject: [PATCH 60/90] fixed bug with IMU bias. added option to delay start of GPS factors --- .../+imuSimulator/covarianceAnalysisBetween.m | 17 +++++++++-------- .../covarianceAnalysisCreateFactorGraph.m | 2 +- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 1419f1773..feb840ad4 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -11,25 +11,26 @@ clc clear all close all -saveResults = 0; +saveResults = 1; %% Configuration options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj -options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses +options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) -options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements +options.imuNonzeroBias = 1; % if true, a nonzero bias is applied to IMU measurements options.includeCameraFactors = 0; % not fully implementemeasurements, values, noiseModels, measurementNoise, options, metadatad yet numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses +options.gpsStartPose = 100; % Pose number to start including GPS factors at options.trajectoryLength = 209; % length of the ground truth trajectory options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) -numMonteCarloRuns = 50; % number of Monte Carlo runs to perform +numMonteCarloRuns = 100; % number of Monte Carlo runs to perform %% Camera metadata K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration @@ -53,8 +54,8 @@ metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); metadata.imu.AccelerometerSigma = 1e-3; metadata.imu.GyroscopeSigma = 1e-5; metadata.imu.IntegrationSigma = 1e-5; -metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; -metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; +metadata.imu.BiasAccelerometerSigma = 1e-4; %metadata.imu.epsBias; +metadata.imu.BiasGyroscopeSigma = 1e-4; %metadata.imu.epsBias; metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias; metadata.imu.g = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0]; @@ -154,8 +155,8 @@ for k=1:numMonteCarloRuns % Create a random bias for each run if options.imuNonzeroBias == 1 - metadata.imu.accelBiasVector = metadata.imu.BiasAccelerometerSigma .* randn(3,1); - metadata.imu.gyroBiasVector = metadata.imu.BiasGyroscopeSigma .* randn(3,1); + metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccelerometerSigma .* randn(3,1); + metadata.imu.gyroConstantBiasVector = metadata.imu.BiasGyroscopeSigma .* randn(3,1); else metadata.imu.accelConstantBiasVector = zeros(3,1); metadata.imu.gyroConstantBiasVector = zeros(3,1); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 921d42679..b134a08d3 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -146,7 +146,7 @@ for i=0:length(measurements) end % end of Camera factor creation %% Add GPS factors - if options.includeGPSFactors == 1 + if options.includeGPSFactors == 1 && i >= options.gpsStartPose gpsPosition = measurements(i).gpsPositionVector ... + measurementNoise.gpsNoiseVector .* randn(3,1); graph.add(PriorFactorPose3(currentPoseKey, ... From 20eeb90682b6c1cdd2534331438930743ed05a65 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 24 Apr 2014 10:24:44 -0400 Subject: [PATCH 61/90] Removed noise from pose prior for non-BetweenFactors. Added saving figures as .png --- .../+imuSimulator/covarianceAnalysisBetween.m | 20 +++++++++++-------- .../covarianceAnalysisCreateFactorGraph.m | 6 +++++- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index feb840ad4..49a1c85ad 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -11,15 +11,15 @@ clc clear all close all -saveResults = 1; +saveResults = 0; %% Configuration options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj -options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses +options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) -options.imuNonzeroBias = 1; % if true, a nonzero bias is applied to IMU measurements +options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements options.includeCameraFactors = 0; % not fully implementemeasurements, values, noiseModels, measurementNoise, options, metadatad yet numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors @@ -30,7 +30,7 @@ options.gpsStartPose = 100; % Pose number to start including GPS factors options.trajectoryLength = 209; % length of the ground truth trajectory options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) -numMonteCarloRuns = 100; % number of Monte Carlo runs to perform +numMonteCarloRuns = 2; % number of Monte Carlo runs to perform %% Camera metadata K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration @@ -54,8 +54,8 @@ metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); metadata.imu.AccelerometerSigma = 1e-3; metadata.imu.GyroscopeSigma = 1e-5; metadata.imu.IntegrationSigma = 1e-5; -metadata.imu.BiasAccelerometerSigma = 1e-4; %metadata.imu.epsBias; -metadata.imu.BiasGyroscopeSigma = 1e-4; %metadata.imu.epsBias; +metadata.imu.BiasAccelerometerSigma = 1e-10; %metadata.imu.epsBias; +metadata.imu.BiasGyroscopeSigma = 1e-10; %metadata.imu.epsBias; metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias; metadata.imu.g = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0]; @@ -151,7 +151,7 @@ monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0]; monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS; for k=1:numMonteCarloRuns - fprintf('Monte Carlo Run %d.\n', k'); + fprintf('Monte Carlo Run %d...', k'); % Create a random bias for each run if options.imuNonzeroBias == 1 @@ -176,7 +176,8 @@ for k=1:numMonteCarloRuns % optimize optimizer = GaussNewtonOptimizer(graph, gtValues); estimate = optimizer.optimize(); - + fprintf('\n\tEstimate error: %g \n', graph.error(estimate) ); + fprintf('\tEstimate error (GT): %g \n', graph.error(gtValues) ); figure(1) plot3DTrajectory(estimate, '-b'); @@ -212,6 +213,7 @@ set(gca,'Fontsize',16) title('NEES and ANEES'); if saveResults saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig'); + saveas(gcf,horzcat(folderName,'runs-',testName,'.png'),'png'); end %% @@ -221,6 +223,7 @@ set(gca,'Fontsize',16) title('Ground truth and estimates for each MC runs'); if saveResults saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig'); + saveas(gcf,horzcat(folderName,'gt-',testName,'.png'),'png'); end %% Let us compute statistics on the overall NEES @@ -248,6 +251,7 @@ set(gca,'Fontsize',16) title('NEES normalized by dof VS bounds'); if saveResults saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig'); + saveas(gcf,horzcat(folderName,'ANEES-',testName,'.png'),'png'); logFile = horzcat(folderName,'log-',testName); save(logFile) end diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index b134a08d3..2f9514b06 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -19,7 +19,11 @@ for i=0:length(measurements) if i==0 %% first time step, add priors % Pose prior (poses used for all factors) - initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); + if options.includeBetweenFactors == 1 + initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); + else + initialPose = Pose3; + end graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); % IMU velocity and bias priors From a077dadc99b64275ba6a2ab8d54959e176768942 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 24 Apr 2014 15:52:11 -0400 Subject: [PATCH 62/90] added new constructor to CombinedImuFactor in matlab wrapper --- gtsam.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam.h b/gtsam.h index 5d86110aa..823a2606d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2325,6 +2325,15 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix biasAccCovariance, Matrix biasOmegaCovariance, Matrix biasAccOmegaInit); + CombinedImuFactorPreintegratedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit, + bool use2ndOrderIntegration); CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); // Testable From 5391de77e0506539a5551106de62b99a02d05722 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 24 Apr 2014 17:01:08 -0400 Subject: [PATCH 63/90] working on IMU biases --- .../+imuSimulator/covarianceAnalysisBetween.m | 67 +++++++++++-------- .../covarianceAnalysisCreateFactorGraph.m | 16 ++--- 2 files changed, 45 insertions(+), 38 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 49a1c85ad..a2a7c3a7c 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -19,18 +19,18 @@ options.includeBetweenFactors = 0; % if true, BetweenFactors will be added betwe options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) -options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements +options.imuNonzeroBias = 1; % if true, a nonzero bias is applied to IMU measurements -options.includeCameraFactors = 0; % not fully implementemeasurements, values, noiseModels, measurementNoise, options, metadatad yet +options.includeCameraFactors = 0; % not fully implemented yet numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses options.gpsStartPose = 100; % Pose number to start including GPS factors at -options.trajectoryLength = 209; % length of the ground truth trajectory +options.trajectoryLength = 50; %209; % length of the ground truth trajectory options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) -numMonteCarloRuns = 2; % number of Monte Carlo runs to perform +numMonteCarloRuns = 20; % number of Monte Carlo runs to perform %% Camera metadata K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration @@ -49,33 +49,43 @@ if options.includeCameraFactors == 1 end %% Imu metadata +sigma_accel = 1e-3; % std. deviation for accelerometer noise, typical 1e-3 +sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5 +sigma_accelBias = 1e-3; % std. deviation for added accelerometer constant bias, typical 1e-3 +sigma_gyroBias = 1e-5; % std. deviation for added gyroscope constant bias, typical 1e-5 + metadata.imu.epsBias = 1e-10; % was 1e-7 -metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); -metadata.imu.AccelerometerSigma = 1e-3; -metadata.imu.GyroscopeSigma = 1e-5; -metadata.imu.IntegrationSigma = 1e-5; -metadata.imu.BiasAccelerometerSigma = 1e-10; %metadata.imu.epsBias; -metadata.imu.BiasGyroscopeSigma = 1e-10; %metadata.imu.epsBias; -metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias; metadata.imu.g = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0]; -noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 -noiseBias = noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias); % between on biases -noisePriorBias = noiseModel.Diagonal.Sigmas([metadata.imu.BiasAccelerometerSigma * ones(3,1); ... - metadata.imu.BiasGyroscopeSigma * ones(3,1)]); +metadata.imu.IntegrationSigma = 1e-5; +metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); +metadata.imu.AccelerometerSigma = sigma_accel; +metadata.imu.GyroscopeSigma = sigma_gyro; +metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; % noise on expected change in accelerometer bias over time +metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; % noise on expected change in gyroscope bias over time +% noise on initial accelerometer and gyroscope biases +if options.imuNonzeroBias == 1 + metadata.imu.BiasAccOmegaInit = [sigma_accelBias * ones(3,1); sigma_gyroBias * ones(3,1)]; +else + metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias * ones(6,1); +end -sigma_accel = metadata.imu.AccelerometerSigma; -sigma_gyro = metadata.imu.GyroscopeSigma; -noiseVectorAccel = sigma_accel * ones(3,1); -noiseVectorGyro = sigma_gyro * ones(3,1); +noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 +noiseBiasBetween = noiseModel.Diagonal.Sigmas([metadata.imu.BiasAccelerometerSigma * ones(3,1);... + metadata.imu.BiasGyroscopeSigma * ones(3,1)]); % between on biases +noisePriorBias = noiseModel.Diagonal.Sigmas(metadata.imu.BiasAccOmegaInit); + +noiseVectorAccel = metadata.imu.AccelerometerSigma * ones(3,1); +noiseVectorGyro = metadata.imu.GyroscopeSigma * ones(3,1); %% Between metadata -sigma_ang = 1e-2; sigma_cart = 1e-1; +sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 +sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1 noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); %% GPS metadata -sigma_gps = 1e-4; +sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4 noiseVectorGPS = sigma_gps * ones(3,1); noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]); @@ -90,7 +100,7 @@ folderName = 'results/' % Set up noise models gtNoiseModels.noisePose = noisePose; gtNoiseModels.noiseVel = noiseVel; -gtNoiseModels.noiseBias = noiseBias; +gtNoiseModels.noiseBiasBetween = noiseBiasBetween; gtNoiseModels.noisePriorPose = noisePose; gtNoiseModels.noisePriorBias = noisePriorBias; gtNoiseModels.noiseGPS = noiseGPS; @@ -138,7 +148,7 @@ disp('Plotted ground truth') % Set up noise models monteCarloNoiseModels.noisePose = noisePose; monteCarloNoiseModels.noiseVel = noiseVel; -monteCarloNoiseModels.noiseBias = noiseBias; +monteCarloNoiseModels.noiseBiasBetween = noiseBiasBetween; monteCarloNoiseModels.noisePriorPose = noisePose; monteCarloNoiseModels.noisePriorBias = noisePriorBias; monteCarloNoiseModels.noiseGPS = noiseGPS; @@ -151,12 +161,14 @@ monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0]; monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS; for k=1:numMonteCarloRuns - fprintf('Monte Carlo Run %d...', k'); + fprintf('Monte Carlo Run %d...\n', k'); % Create a random bias for each run if options.imuNonzeroBias == 1 - metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccelerometerSigma .* randn(3,1); - metadata.imu.gyroConstantBiasVector = metadata.imu.BiasGyroscopeSigma .* randn(3,1); + %metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccOmegaInit(1:3) .* randn(3,1); + %metadata.imu.gyroConstantBiasVector = metadata.imu.BiasAccOmegaInit(4:6) .* randn(3,1); + metadata.imu.accelConstantBiasVector = 1e-2 * ones(3,1); + metadata.imu.gyroConstantBiasVector = 1e-3 * ones(3,1); else metadata.imu.accelConstantBiasVector = zeros(3,1); metadata.imu.gyroConstantBiasVector = zeros(3,1); @@ -176,8 +188,6 @@ for k=1:numMonteCarloRuns % optimize optimizer = GaussNewtonOptimizer(graph, gtValues); estimate = optimizer.optimize(); - fprintf('\n\tEstimate error: %g \n', graph.error(estimate) ); - fprintf('\tEstimate error (GT): %g \n', graph.error(gtValues) ); figure(1) plot3DTrajectory(estimate, '-b'); @@ -195,7 +205,6 @@ for k=1:numMonteCarloRuns % compute covariances: cov = marginals.marginalCovariance(currentPoseKey); covPosition = estR * cov(4:6,4:6) * estR'; - % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances NEES(k,i+1) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof end diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 2f9514b06..038a0eb04 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -19,11 +19,7 @@ for i=0:length(measurements) if i==0 %% first time step, add priors % Pose prior (poses used for all factors) - if options.includeBetweenFactors == 1 - initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); - else - initialPose = Pose3; - end + initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); % IMU velocity and bias priors @@ -90,19 +86,21 @@ for i=0:length(measurements) currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); % Add between factor on biases graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... - noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); + noiseModels.noiseBiasBetween)); end if options.imuFactorType == 2 + use2ndOrderIntegration = true; % Initialize preintegration imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(... metadata.imu.zeroBias, ... metadata.imu.AccelerometerSigma.^2 * eye(3), ... metadata.imu.GyroscopeSigma.^2 * eye(3), ... metadata.imu.IntegrationSigma.^2 * eye(3), ... - metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... - metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... - metadata.imu.BiasAccOmegaInit.^2 * eye(6)); + metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... % how bias changes over time + metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... % how bias changes over time + diag(metadata.imu.BiasAccOmegaInit.^2), ... % prior on bias + use2ndOrderIntegration); % Generate IMU measurements with noise for j=1:length(measurements(i).imu) % all measurements to preintegrate imuAccel = measurements(i).imu(j).accel ... From 4ad4f9d68e7535dee0b9e06cfe9d056ff5cf2ab2 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Tue, 29 Apr 2014 15:13:28 -0400 Subject: [PATCH 64/90] removed unnecessary parameter from constructor --- gtsam.h | 3 +-- gtsam/navigation/CombinedImuFactor.h | 4 ++-- gtsam/navigation/tests/testCombinedImuFactor.cpp | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam.h b/gtsam.h index 823a2606d..9aa19db10 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2347,8 +2347,7 @@ class CombinedImuFactorPreintegratedMeasurements { virtual class CombinedImuFactor : gtsam::NonlinearFactor { CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, - const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis, - const gtsam::noiseModel::Base* model); + const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); // Standard Interface gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index bd0b9c03b..7d0e78a6f 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -341,8 +341,8 @@ namespace gtsam { /** Constructor */ CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : - Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index a07539d1e..1a8b6160d 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -183,7 +183,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis, Combinedmodel); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); From 862f5c7af3cae1c7ce7caf7ef52eefa248413f8d Mon Sep 17 00:00:00 2001 From: djensen3 Date: Tue, 29 Apr 2014 15:46:43 -0400 Subject: [PATCH 65/90] changed timestep to make IMU measurements more realistic. removed noise model from IMU type 2 constructor --- .../+imuSimulator/covarianceAnalysisBetween.m | 19 +++++++-------- .../covarianceAnalysisCreateFactorGraph.m | 23 +++++++++++++++---- .../covarianceAnalysisCreateTrajectory.m | 2 +- 3 files changed, 30 insertions(+), 14 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index a2a7c3a7c..4f60ae2ef 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -2,7 +2,7 @@ import gtsam.*; % Test GTSAM covariances on a factor graph with: % Between Factors -% IMU factors +% IMU factors (type 1 and type 2) % Projection factors % Authors: Luca Carlone, David Jensen % Date: 2014/4/6 @@ -18,7 +18,7 @@ options.useRealData = 1; % controls whether or not to use the real dat options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) -options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) +options.imuFactorType = 2; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) options.imuNonzeroBias = 1; % if true, a nonzero bias is applied to IMU measurements options.includeCameraFactors = 0; % not fully implemented yet @@ -27,7 +27,7 @@ numberOfLandmarks = 10; % Total number of visual landmarks, used for options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses options.gpsStartPose = 100; % Pose number to start including GPS factors at -options.trajectoryLength = 50; %209; % length of the ground truth trajectory +options.trajectoryLength = 209;%209; % length of the ground truth trajectory options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) numMonteCarloRuns = 20; % number of Monte Carlo runs to perform @@ -51,8 +51,8 @@ end %% Imu metadata sigma_accel = 1e-3; % std. deviation for accelerometer noise, typical 1e-3 sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5 -sigma_accelBias = 1e-3; % std. deviation for added accelerometer constant bias, typical 1e-3 -sigma_gyroBias = 1e-5; % std. deviation for added gyroscope constant bias, typical 1e-5 +sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3 +sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5 metadata.imu.epsBias = 1e-10; % was 1e-7 metadata.imu.g = [0;0;0]; @@ -132,6 +132,7 @@ figure(1) hold on; plot3DPoints(gtValues); plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); +%plot3DTrajectory(gtValues, '-r'); axis equal % optimize @@ -165,10 +166,10 @@ for k=1:numMonteCarloRuns % Create a random bias for each run if options.imuNonzeroBias == 1 - %metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccOmegaInit(1:3) .* randn(3,1); - %metadata.imu.gyroConstantBiasVector = metadata.imu.BiasAccOmegaInit(4:6) .* randn(3,1); - metadata.imu.accelConstantBiasVector = 1e-2 * ones(3,1); - metadata.imu.gyroConstantBiasVector = 1e-3 * ones(3,1); + metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccOmegaInit(1:3) .* randn(3,1); + metadata.imu.gyroConstantBiasVector = metadata.imu.BiasAccOmegaInit(4:6) .* randn(3,1); + %metadata.imu.accelConstantBiasVector = 1e-2 * ones(3,1); + %metadata.imu.gyroConstantBiasVector = 1e-3 * ones(3,1); else metadata.imu.accelConstantBiasVector = zeros(3,1); metadata.imu.gyroConstantBiasVector = zeros(3,1); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 038a0eb04..d87fa490b 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -69,13 +69,28 @@ for i=0:length(measurements) use2ndOrderIntegration); % Generate IMU measurements with noise for j=1:length(measurements(i).imu) % all measurements to preintegrate + accelNoise = (measurementNoise.imu.accelNoiseVector .* randn(3,1)); imuAccel = measurements(i).imu(j).accel ... - + (measurementNoise.imu.accelNoiseVector .* randn(3,1))... % added noise + + accelNoise ... % added noise + metadata.imu.accelConstantBiasVector; % constant bias + + gyroNoise = (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); imuGyro = measurements(i).imu(j).gyro ... - + (measurementNoise.imu.gyroNoiseVector .* randn(3,1))... % added noise + + gyroNoise ... % added noise + metadata.imu.gyroConstantBiasVector; % constant bias + % Used for debugging + %fprintf(' A: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ... + % measurements(i).imu(j).accel(1), measurements(i).imu(j).accel(2), measurements(i).imu(j).accel(3), ... + % accelNoise(1), accelNoise(2), accelNoise(3), ... + % metadata.imu.accelConstantBiasVector(1), metadata.imu.accelConstantBiasVector(2), metadata.imu.accelConstantBiasVector(3), ... + % imuAccel(1), imuAccel(2), imuAccel(3)); + %fprintf(' G: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ... + % measurements(i).imu(j).gyro(1), measurements(i).imu(j).gyro(2), measurements(i).imu(j).gyro(3), ... + % gyroNoise(1), gyroNoise(2), gyroNoise(3), ... + % metadata.imu.gyroConstantBiasVector(1), metadata.imu.gyroConstantBiasVector(2), metadata.imu.gyroConstantBiasVector(3), ... + % imuGyro(1), imuGyro(2), imuGyro(3)); + % Preintegrate imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT); end @@ -113,14 +128,14 @@ for i=0:length(measurements) % Preintegrate imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT); end + % Add Imu factor graph.add(CombinedImuFactor( ... currentPoseKey-1, currentVelKey-1, ... currentPoseKey, currentVelKey, ... currentBiasKey-1, currentBiasKey, ... imuMeasurement, ... - metadata.imu.g, metadata.imu.omegaCoriolis, ... - noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias))); + metadata.imu.g, metadata.imu.omegaCoriolis)); end end % end of IMU factor creation diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 3933870b5..b66df3fc9 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -43,7 +43,7 @@ if options.useRealData == 1 if options.includeIMUFactors == 1 currentVelKey = symbol('v', i); currentBiasKey = symbol('b', i); - deltaT = 0.01; % amount of time between IMU measurements + deltaT = 1; % amount of time between IMU measurements if i == 0 currentVel = [0 0 0]'; else From 2cb448fa2432d1e033567e018f0235af0e5428ba Mon Sep 17 00:00:00 2001 From: djensen3 Date: Wed, 30 Apr 2014 16:17:32 -0400 Subject: [PATCH 66/90] adding SmartProjectionPoseFactor to wrapper --- gtsam_unstable/gtsam_unstable.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index d87e71048..44f30e572 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -376,19 +376,19 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; -//#include -//template -//virtual class SmartProjectionPoseFactor: public SmartProjectionFactor { +#include +template +virtual class SmartProjectionPoseFactor: gtsam::SmartProjectionFactor { -// SmartProjectionPoseFactor(const double rankTol, const double linThreshold, -// const bool manageDegeneracy, const bool enableEPI, const POSE& body_P_sensor); -// -// void add(const Point2 measured_i, const Key poseKey_i, const SharedNoiseModel noise_i, -// const CALIBRATION* K_i); -// -// // enabling serialization functionality -// void serialize() const; -//}; + SmartProjectionPoseFactor(double rankTol, double linThreshold, + bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + + void add(const Point2 measured_i, const Key poseKey_i, const SharedNoiseModel noise_i, + const CALIBRATION* K_i); + + // enabling serialization functionality + void serialize() const; +}; #include From 32e1a8f9949be9be77a0d3eae990fb4c1ab668f5 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 1 May 2014 10:48:54 -0400 Subject: [PATCH 67/90] tried some changes for wrapping --- gtsam_unstable/gtsam_unstable.h | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 44f30e572..0e2ef44b1 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -377,19 +377,20 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; #include -template -virtual class SmartProjectionPoseFactor: gtsam::SmartProjectionFactor { +template +virtual class SmartProjectionPoseFactor : gtsam::SmartProjectionFactor { - SmartProjectionPoseFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + //SmartProjectionPoseFactor(double rankTol, double linThreshold, bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + SmartProjectionPoseFactor(); - void add(const Point2 measured_i, const Key poseKey_i, const SharedNoiseModel noise_i, - const CALIBRATION* K_i); + //void add(gtsam::Point2 measured_i, size_t poseKey_i, gtsam::SharedNoiseModel noise_i, CALIBRATION* K_i); // enabling serialization functionality - void serialize() const; + // void serialize() const; }; +//typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; + #include template From ce13807d10611e0861caf50092363203acc31b56 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 1 May 2014 12:56:24 -0400 Subject: [PATCH 68/90] added script to run and save tests in a simplified way --- gtsam_unstable/gtsam_unstable.h | 27 ++- .../+imuSimulator/covarianceAnalysisBetween.m | 115 ++++++------ .../+imuSimulator/runConsistencyTests.m | 172 ++++++++++++++++++ 3 files changed, 257 insertions(+), 57 deletions(-) create mode 100644 matlab/unstable_examples/+imuSimulator/runConsistencyTests.m diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 0e2ef44b1..c0bf66f37 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -20,6 +20,8 @@ virtual class gtsam::NonlinearFactor; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; +virtual class gtsam::SmartFactorBase; +virtual class gtsam::SmartProjectionFactor; class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; class gtsam::Ordering; @@ -376,6 +378,26 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; +#include +template +virtual class SmartFactorBase : gtsam::NonlinearFactor { + SmartFactorBase(const POSE& body_P_sensor); + + void add(const Point2& measured_i, const Key& poseKey_i, + const SharedNoiseModel& noise_i); + + void serialize() const; +}; + +#include +template +virtual class SmartProjectionFactor : gtsam::SmartFactorBase { + SmartProjectionFactor(double rankTol, double linThreshold, + bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + + void serialize() const; +}; + #include template virtual class SmartProjectionPoseFactor : gtsam::SmartProjectionFactor { @@ -386,12 +408,9 @@ virtual class SmartProjectionPoseFactor : gtsam::SmartProjectionFactor { //void add(gtsam::Point2 measured_i, size_t poseKey_i, gtsam::SharedNoiseModel noise_i, CALIBRATION* K_i); // enabling serialization functionality - // void serialize() const; + void serialize() const; }; -//typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - - #include template virtual class RangeFactor : gtsam::NonlinearFactor { diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 4f60ae2ef..1c9311a93 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -7,53 +7,56 @@ import gtsam.*; % Authors: Luca Carlone, David Jensen % Date: 2014/4/6 -clc -clear all -close all -saveResults = 0; - -%% Configuration -options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj -options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses - -options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) -options.imuFactorType = 2; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) -options.imuNonzeroBias = 1; % if true, a nonzero bias is applied to IMU measurements - -options.includeCameraFactors = 0; % not fully implemented yet -numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors - -options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses -options.gpsStartPose = 100; % Pose number to start including GPS factors at - -options.trajectoryLength = 209;%209; % length of the ground truth trajectory -options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) - -numMonteCarloRuns = 20; % number of Monte Carlo runs to perform - -%% Camera metadata -K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration -cameraMeasurementNoiseSigma = 1.0; -cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); - -% Create landmarks -if options.includeCameraFactors == 1 - for i = 1:numberOfLandmarks - gtLandmarkPoints(i) = Point3( ... - ... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses - [rand()*20*(options.trajectoryLength*1.2) + 15*20; ... - randn()*20; ... % normally distributed in the y axis with a sigma of 20 - randn()*20]); % normally distributed in the z axis with a sigma of 20 - end +% Check for an extneral configuration, used when running multiple tests +if ~exist('externallyConfigured', 'var') + clc + clear all + close all + + saveResults = 0; + + %% Configuration + % General options + options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj + options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses + + options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) + options.imuFactorType = 2; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) + options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements + + options.includeCameraFactors = 0; % not fully implemented yet + numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors + + options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses + options.gpsStartPose = 100; % Pose number to start including GPS factors at + + options.trajectoryLength = 209;%209; % length of the ground truth trajectory + options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) + + numMonteCarloRuns = 5; % number of Monte Carlo runs to perform + + % Noise values to be adjusted + sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 + sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1 + sigma_accel = 1e-1; % std. deviation for accelerometer noise, typical 1e-3 + sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5 + sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3 + sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5 + sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4 + + % Set log files + testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro) + folderName = 'results/' +else + fprintf('Tests have been externally configured.\n'); end -%% Imu metadata -sigma_accel = 1e-3; % std. deviation for accelerometer noise, typical 1e-3 -sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5 -sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3 -sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5 +%% Between metadata +noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; +noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); +%% Imu metadata metadata.imu.epsBias = 1e-10; % was 1e-7 metadata.imu.g = [0;0;0]; metadata.imu.omegaCoriolis = [0;0;0]; @@ -78,20 +81,26 @@ noisePriorBias = noiseModel.Diagonal.Sigmas(metadata.imu.BiasAccOmegaInit); noiseVectorAccel = metadata.imu.AccelerometerSigma * ones(3,1); noiseVectorGyro = metadata.imu.GyroscopeSigma * ones(3,1); -%% Between metadata -sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 -sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1 -noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; -noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); - %% GPS metadata -sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4 noiseVectorGPS = sigma_gps * ones(3,1); noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]); -%% Set log files -testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro) -folderName = 'results/' +%% Camera metadata +K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration +cameraMeasurementNoiseSigma = 1.0; +cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); + +% Create landmarks +if options.includeCameraFactors == 1 + for i = 1:numberOfLandmarks + gtLandmarkPoints(i) = Point3( ... + ... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses + [rand()*20*(options.trajectoryLength*1.2) + 15*20; ... + randn()*20; ... % normally distributed in the y axis with a sigma of 20 + randn()*20]); % normally distributed in the z axis with a sigma of 20 + end +end + %% Create ground truth trajectory and measurements [gtValues, gtMeasurements] = imuSimulator.covarianceAnalysisCreateTrajectory(options, metadata); diff --git a/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m new file mode 100644 index 000000000..818a41388 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m @@ -0,0 +1,172 @@ +% use this script to easily run and save results for multiple consistency +% tests without having to pay attention to the computer every 5 minutes + +import gtsam.*; + +resultsDir = 'results/' +if (~exist(resultsDir, 'dir')) + mkdir(resultsDir); +end + +testOptions = [ ... + % 1 2 3 4 5 6 7 8 9 10 11 12 + % RealData? Between? IMU? IMUType Bias? Camera? #LndMrk GPS? StrtPose TrajLength Subsample #MCRuns + %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 1 + %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 2 + % 1 0 1 2 0 0 10 0 100 209 20 100 ;... % 3 + %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 4 + %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 5 + %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 6 + % 1 0 1 2 0 0 10 0 100 209 20 100 ;... % 7 + 1 0 1 2 1 0 10 0 100 209 20 100 ;... % 8 + 1 0 1 2 1 0 10 0 100 209 20 100 ]; % 9 + +noises = [ ... + % 1 2 3 4 5 6 7 + % sigma_ang sigma_cart sigma_accel sigma_gyro sigma_accelBias sigma_gyroBias sigma_gps + %1e-2 1e-1 1e-3 1e-5 0 0 1e-4;... % 1 + %1e-2 1e-1 1e-2 1e-5 0 0 1e-4;... % 2 + % 1e-2 1e-1 1e-1 1e-5 0 0 1e-4;... % 3 + %1e-2 1e-1 1e-3 1e-4 0 0 1e-4;... % 4 + %1e-2 1e-1 1e-3 1e-3 0 0 1e-4;... % 5 + %1e-2 1e-1 1e-3 1e-2 0 0 1e-4;... % 6 + % 1e-2 1e-1 1e-3 1e-1 0 0 1e-4;... % 7 + 1e-2 1e-1 1e-3 1e-5 1e-3 1e-5 1e-4;... % 8 + 1e-2 1e-1 1e-3 1e-5 1e-4 1e-6 1e-4]; % 9 + +if(size(testOptions,1) ~= size(noises,1)) + error('testOptions and noises do not have same number of rows'); +end + +% Set flag so the script knows there is an external configuration +externallyConfigured = 1; + +% Set the flag to save the results +saveResults = 1; + +errorRuns = []; + +% Go through tests +for i = 1:size(testOptions,1) + % Clean up from last test + close all; + %clc; + + % Set up variables for test + options.useRealData = testOptions(i,1); + options.includeBetweenFactors = testOptions(i,2); + options.includeIMUFactors = testOptions(i,3); + options.imuFactorType = testOptions(i,4); + options.imuNonzeroBias = testOptions(i,5); + options.includeCameraFactors = testOptions(i,6); + numberOfLandmarks = testOptions(i,7); + options.includeGPSFactors = testOptions(i,8); + options.gpsStartPose = testOptions(i,9); + options.trajectoryLength = testOptions(i,10); + options.subsampleStep = testOptions(i,11); + numMonteCarloRuns = testOptions(i,12); + + sigma_ang = noises(i,1); + sigma_cart = noises(i,2); + sigma_accel = noises(i,3); + sigma_gyro = noises(i,4); + sigma_accelBias = noises(i,5); + sigma_gyroBias = noises(i,6); + sigma_gps = noises(i,7); + + % Create folder name + f_between = ''; + f_imu = ''; + f_bias = ''; + f_gps = ''; + f_camera = ''; + f_runs = ''; + + if (options.includeBetweenFactors == 1); + f_between = 'between_'; + end + if (options.includeIMUFactors == 1) + f_imu = sprintf('imu%d_', options.imuFactorType); + if (options.imuNonzeroBias == 1); + f_bias = sprintf('bias_a%1.2g_g%1.2g_', sigma_accelBias, sigma_gyroBias); + end + end + if (options.includeGPSFactors == 1); + f_between = sprintf('gps_%d_', gpsStartPose); + end + if (options.includeCameraFactors == 1) + f_camera = sprintf('camera_%d_', numberOfLandmarks); + end + f_runs = sprintf('mc%d', numMonteCarloRuns); + + folderName = [resultsDir f_between f_imu f_bias f_gps f_camera f_runs '/']; + + % make folder if it doesnt exist + if (~exist(folderName, 'dir')) + mkdir(folderName); + end + + testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro); + + % Run the test + fprintf('Test %d\n\tResults will be saved to:\n\t%s\n\trunning...\n', i, folderName); + fprintf('Test Name: %s\n', testName); + + try + imuSimulator.covarianceAnalysisBetween; + catch + errorRuns = [errorRuns i]; + fprintf('\n*****\n Something went wrong, most likely indeterminant linear system error.\n'); + disp('Test Options:\n'); + disp(testOptions(i,:)); + disp('Noises'); + disp(noises(i,:)); + fprintf('\n*****\n\n'); + end +end + +% Print error summary +fprintf('*************************\n'); +fprintf('%d Runs failed due to errors (data not collected for failed runs)\n', length(errorRuns)); +for i = 1:length(errorRuns) + k = errorRuns(i); + fprintf('\nTest %d:\n', k); + fprintf(' options.useRealData = %d\n', testOptions(k,1)); + fprintf(' options.includeBetweenFactors = %d\n', testOptions(k,2)); + fprintf(' options.includeIMUFactors = %d\n', testOptions(k,3)); + fprintf(' options.imuFactorType = %d\n', testOptions(k,4)); + fprintf(' options.imuNonzeroBias = %d\n', testOptions(k,5)); + fprintf(' options.includeCameraFactors = %d\n', testOptions(k,6)); + fprintf(' numberOfLandmarks = %d\n', testOptions(k,7)); + fprintf(' options.includeGPSFactors = %d\n', testOptions(k,8)); + fprintf(' options.gpsStartPose = %d\n', testOptions(k,9)); + fprintf(' options.trajectoryLength = %d\n', testOptions(k,10)); + fprintf(' options.subsampleStep = %d\n', testOptions(k,11)); + fprintf(' numMonteCarloRuns = %d\n', testOptions(k,12)); + fprintf('\n'); + fprintf(' sigma_ang = %f\n', noises(i,1)); + fprintf(' sigma_cart = %f\n', noises(i,2)); + fprintf(' sigma_accel = %f\n', noises(i,3)); + fprintf(' sigma_gyro = %f\n', noises(i,4)); + fprintf(' sigma_accelBias = %f\n', noises(i,5)); + fprintf(' sigma_gyroBias = %f\n', noises(i,6)); + fprintf(' sigma_gps = %f\n', noises(i,7)); +end + + + + + + + + + + + + + + + + + + From 8dbd7c175c841b0b3058bda93dc9e995ee4865b5 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 1 May 2014 15:55:37 -0400 Subject: [PATCH 69/90] added SmartProjectionFactor to matlab wrapper --- gtsam_unstable/gtsam_unstable.h | 40 ++++++++++----------------------- 1 file changed, 12 insertions(+), 28 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index c0bf66f37..5b173cb85 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -20,8 +20,8 @@ virtual class gtsam::NonlinearFactor; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; -virtual class gtsam::SmartFactorBase; -virtual class gtsam::SmartProjectionFactor; +virtual class gtsam::SharedNoiseModel; +virtual class gtsam::Cal3_S2; class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; class gtsam::Ordering; @@ -378,39 +378,23 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; -#include -template -virtual class SmartFactorBase : gtsam::NonlinearFactor { - SmartFactorBase(const POSE& body_P_sensor); - - void add(const Point2& measured_i, const Key& poseKey_i, - const SharedNoiseModel& noise_i); - - void serialize() const; -}; - -#include -template -virtual class SmartProjectionFactor : gtsam::SmartFactorBase { - SmartProjectionFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); - - void serialize() const; -}; - #include -template -virtual class SmartProjectionPoseFactor : gtsam::SmartProjectionFactor { +template +virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { - //SmartProjectionPoseFactor(double rankTol, double linThreshold, bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); - SmartProjectionPoseFactor(); + SmartProjectionPoseFactor(double rankTol, double linThreshold, + bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + //SmartProjectionPoseFactor(); - //void add(gtsam::Point2 measured_i, size_t poseKey_i, gtsam::SharedNoiseModel noise_i, CALIBRATION* K_i); + void add(const gtsam::Point2 measured_i, const gtsam::Key poseKey_i, const gtsam::SharedNoiseModel noise_i, + const CALIBRATION* K_i); // enabling serialization functionality - void serialize() const; + //void serialize() const; }; +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; + #include template virtual class RangeFactor : gtsam::NonlinearFactor { From 5402f9fa202b23fa8a20207d8997331162b68a9d Mon Sep 17 00:00:00 2001 From: djensen3 Date: Fri, 2 May 2014 11:12:10 -0400 Subject: [PATCH 70/90] Removed smartProjectionPoseFactor from matlab wrapper; it was causing matlab crashes --- gtsam_unstable/gtsam_unstable.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 5b173cb85..440b26388 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -377,13 +377,15 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { void print(string s) const; }; - +/* #include template virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { SmartProjectionPoseFactor(double rankTol, double linThreshold, bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + + SmartProjectionPoseFactor(double rankTol); //SmartProjectionPoseFactor(); void add(const gtsam::Point2 measured_i, const gtsam::Key poseKey_i, const gtsam::SharedNoiseModel noise_i, @@ -394,7 +396,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { }; typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - +*/ #include template virtual class RangeFactor : gtsam::NonlinearFactor { From 0f03cd736cf2abfec5ec1b72841d07ff22587288 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 8 May 2014 11:08:43 -0400 Subject: [PATCH 71/90] added comments --- gtsam/navigation/CombinedImuFactor.h | 16 +++++++++++++--- gtsam/navigation/ImuFactor.h | 15 ++++++++++++--- 2 files changed, 25 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 7d0e78a6f..d4a2d9d12 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -339,9 +339,19 @@ namespace gtsam { CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} /** Constructor */ - CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : + CombinedImuFactor( + Key pose_i, ///< previous pose key + Key vel_i, ///< previous velocity key + Key pose_j, ///< current pose key + Key vel_j, ///< current velocity key + Key bias_i, ///< previous bias key + Key bias_j, ///< current bias key + const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements + const Vector3& gravity, ///< gravity vector + const Vector3& omegaCoriolis, ///< rotation rate of inertial frame + boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame + const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect + ) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 32d099c3f..980b50feb 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -301,9 +301,18 @@ namespace gtsam { ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} /** Constructor */ - ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : + ImuFactor( + Key pose_i, ///< previous pose key + Key vel_i, ///< previous velocity key + Key pose_j, ///< current pose key + Key vel_j, ///< current velocity key + Key bias, ///< previous bias key + const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements + const Vector3& gravity, ///< gravity vector + const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame + boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame + const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect + ) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), From 49b3836c49f0e2e827b4ddcdc493f73d13134f96 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 8 May 2014 15:27:32 -0400 Subject: [PATCH 72/90] added infrastructure for SmartProjectionPose3Factors --- .../+imuSimulator/covarianceAnalysisBetween.m | 61 +++++++++++-------- .../covarianceAnalysisCreateFactorGraph.m | 39 ++++++------ .../covarianceAnalysisCreateTrajectory.m | 15 ++++- .../+imuSimulator/runConsistencyTests.m | 47 +++++++------- 4 files changed, 89 insertions(+), 73 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 1c9311a93..e6149b785 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -3,7 +3,8 @@ import gtsam.*; % Test GTSAM covariances on a factor graph with: % Between Factors % IMU factors (type 1 and type 2) -% Projection factors +% GPS prior factors on poses +% SmartProjectionPoseFactors % Authors: Luca Carlone, David Jensen % Date: 2014/4/6 @@ -19,31 +20,32 @@ if ~exist('externallyConfigured', 'var') %% Configuration % General options options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj - options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses + options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses - options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) - options.imuFactorType = 2; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) + options.includeIMUFactors = 0; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) + options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements - options.includeCameraFactors = 0; % not fully implemented yet - numberOfLandmarks = 10; % Total number of visual landmarks, used for camera factors + options.includeCameraFactors = 1; % if true, SmartProjectionPose3Factors will be used with randomly generated landmarks + options.numberOfLandmarks = 100; % Total number of visual landmarks (randomly generated in a box around the trajectory) options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses options.gpsStartPose = 100; % Pose number to start including GPS factors at - options.trajectoryLength = 209;%209; % length of the ground truth trajectory + options.trajectoryLength = 50;%209; % length of the ground truth trajectory options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) - numMonteCarloRuns = 5; % number of Monte Carlo runs to perform + numMonteCarloRuns = 2; % number of Monte Carlo runs to perform % Noise values to be adjusted sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1 - sigma_accel = 1e-1; % std. deviation for accelerometer noise, typical 1e-3 - sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5 - sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3 - sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5 + sigma_accel = 1e-3; % std. deviation for accelerometer noise, typical 1e-3 + sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5 + sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3 + sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5 sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4 + sigma_camera = 1; % std. deviation for noise in camera measurements (pixels) % Set log files testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro) @@ -86,18 +88,21 @@ noiseVectorGPS = sigma_gps * ones(3,1); noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]); %% Camera metadata -K = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration -cameraMeasurementNoiseSigma = 1.0; -cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2,cameraMeasurementNoiseSigma); +metadata.camera.calibration = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration +metadata.camera.xlims = [-100, 650]; % x limits on area for landmark creation +metadata.camera.ylims = [-100, 700]; % y limits on area for landmark creation +metadata.camera.zlims = [-10, 30]; % z limits on area for landmark creation +metadata.camera.CameraSigma = sigma_camera; +cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2, metadata.camera.CameraSigma); +noiseVectorCamera = metadata.camera.CameraSigma .* ones(2,1); -% Create landmarks +% Create landmarks and smart factors if options.includeCameraFactors == 1 - for i = 1:numberOfLandmarks - gtLandmarkPoints(i) = Point3( ... - ... % uniformly distributed in the x axis along 120% of the trajectory length, starting after 15 poses - [rand()*20*(options.trajectoryLength*1.2) + 15*20; ... - randn()*20; ... % normally distributed in the y axis with a sigma of 20 - randn()*20]); % normally distributed in the z axis with a sigma of 20 + for i = 1:options.numberOfLandmarks + metadata.camera.gtLandmarkPoints(i) = Point3( ... + [rand() * (metadata.camera.xlims(2)-metadata.camera.xlims(1)) + metadata.camera.xlims(1); ... + rand() * (metadata.camera.ylims(2)-metadata.camera.ylims(1)) + metadata.camera.ylims(1); + rand() * (metadata.camera.zlims(2)-metadata.camera.zlims(1)) + metadata.camera.zlims(1)]); end end @@ -113,12 +118,13 @@ gtNoiseModels.noiseBiasBetween = noiseBiasBetween; gtNoiseModels.noisePriorPose = noisePose; gtNoiseModels.noisePriorBias = noisePriorBias; gtNoiseModels.noiseGPS = noiseGPS; +gtNoiseModels.noiseCamera = cameraMeasurementNoise; % Set measurement noise to 0, because this is ground truth gtMeasurementNoise.poseNoiseVector = zeros(6,1); gtMeasurementNoise.imu.accelNoiseVector = zeros(3,1); gtMeasurementNoise.imu.gyroNoiseVector = zeros(3,1); -gtMeasurementNoise.cameraPixelNoiseVector = zeros(2,1); +gtMeasurementNoise.cameraNoiseVector = zeros(2,1); gtMeasurementNoise.gpsNoiseVector = zeros(3,1); % Set IMU biases to zero @@ -140,8 +146,8 @@ gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... figure(1) hold on; plot3DPoints(gtValues); -plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); -%plot3DTrajectory(gtValues, '-r'); +%plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); +plot3DTrajectory(gtValues, '-r'); axis equal % optimize @@ -162,13 +168,14 @@ monteCarloNoiseModels.noiseBiasBetween = noiseBiasBetween; monteCarloNoiseModels.noisePriorPose = noisePose; monteCarloNoiseModels.noisePriorBias = noisePriorBias; monteCarloNoiseModels.noiseGPS = noiseGPS; +monteCarloNoiseModels.noiseCamera = cameraMeasurementNoise; % Set measurement noise for monte carlo runs -monteCarloMeasurementNoise.poseNoiseVector = noiseVectorPose; +monteCarloMeasurementNoise.poseNoiseVector = zeros(6,1); %noiseVectorPose; monteCarloMeasurementNoise.imu.accelNoiseVector = noiseVectorAccel; monteCarloMeasurementNoise.imu.gyroNoiseVector = noiseVectorGyro; -monteCarloMeasurementNoise.cameraPixelNoiseVector = [0; 0]; monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS; +monteCarloMeasurementNoise.cameraNoiseVector = noiseVectorCamera; for k=1:numMonteCarloRuns fprintf('Monte Carlo Run %d...\n', k'); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index d87fa490b..4ce15ced2 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -33,11 +33,12 @@ for i=0:length(measurements) graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias)); end - % Camera priors + %% Create a SmartProjectionFactor for each landmark if options.includeCameraFactors == 1 - pointNoiseSigma = 0.1; - pointPriorNoise = noiseModel.Isotropic.Sigma(3,pointNoiseSigma); - graph.add(PriorFactorPoint3(symbol('p',1), gtLandmarkPoints(1), pointPriorNoise)); + for j=1:options.numberOfLandmarks + %% UNCOMMENT WHEN SMART FACTORS ARE ADDED TO MATLAB WRAPPER + %SmartProjectionFactors(j) = SmartProjectionPose3Factor(); + end end else @@ -140,26 +141,20 @@ for i=0:length(measurements) end % end of IMU factor creation - %% Add Camera factors - UNDER CONSTRUCTION !!!! - - if options.includeCameraFactors == 1 - % Create camera with the current pose and calibration K (specified above) - gtCamera = SimpleCamera(currentPose, K); - % Project landmarks into the camera - numSkipped = 0; - for j = 1:length(gtLandmarkPoints) - landmarkKey = symbol('p', j); - try - Z = gtCamera.project(gtLandmarkPoints(j)); - % TO-DO probably want to do some type of filtering on the measurement values, because - % they might not all be valid - graph.add(GenericProjectionFactorCal3_S2(Z, cameraMeasurementNoise, currentPoseKey, landmarkKey, K)); - catch - % Most likely the point is not within the camera's view, which - % is fine - numSkipped = numSkipped + 1; + %% Add Camera Factors + if options.includeCameraFactors == 1 + for(j = 1:length(measurements(i).landmarks)) + cameraMeasurmentNoise = measurementNoise.cameraNoiseVector .* randn(2,1); + cameraPixelMeasurement = measurements(i).landmarks(j).vector; + if(cameraPixelMeasurement(1) ~= 0 && cameraPixelMeasurement(2) ~= 0) + cameraPixelMeasurement = cameraPixelMeasurement + cameraMeasurmentNoise; + %% UNCOMMENT WHEN SMART FACTORS ARE ADDED TO MATLAB WRAPPER + % SmartProjectionFactors(j).add( ... + % Point2(cameraPizelMeasurement), ... + % currentPoseKey, noiseModels.noiseCamera, ... + % metadata.camera.calibration); end end - %fprintf('(Pose %d) %d landmarks behind the camera\n', i, numSkipped); end % end of Camera factor creation %% Add GPS factors diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index b66df3fc9..be5991a31 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -18,7 +18,7 @@ if options.useRealData == 1 'VEast', 'VNorth', 'VUp'); % Limit the trajectory length - options.trajectoryLength = min([length(gtScenario.Lat) options.trajectoryLength]); + options.trajectoryLength = min([length(gtScenario.Lat)/options.subsampleStep options.trajectoryLength]); fprintf('Scenario Ind: '); for i=0:options.trajectoryLength @@ -86,6 +86,19 @@ if options.useRealData == 1 measurements(i).gpsPositionVector = gpsPositionVector; end + %% gt Camera measurements + if options.includeCameraFactors == 1 && i > 0 + camera = SimpleCamera(currentPose, metadata.camera.calibration); + for j=1:length(metadata.camera.gtLandmarkPoints) + try + z = camera.project(metadata.camera.gtLandmarkPoints(j)); + measurements(i).landmarks(j) = z; + catch + % point is probably out of the camera's view + end + end + end + end fprintf('\n'); else diff --git a/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m index 818a41388..697b49a6a 100644 --- a/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m +++ b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m @@ -11,28 +11,28 @@ end testOptions = [ ... % 1 2 3 4 5 6 7 8 9 10 11 12 % RealData? Between? IMU? IMUType Bias? Camera? #LndMrk GPS? StrtPose TrajLength Subsample #MCRuns - %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 1 - %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 2 - % 1 0 1 2 0 0 10 0 100 209 20 100 ;... % 3 - %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 4 - %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 5 - %1 0 1 2 0 0 10 0 100 209 20 100 ;... % 6 - % 1 0 1 2 0 0 10 0 100 209 20 100 ;... % 7 - 1 0 1 2 1 0 10 0 100 209 20 100 ;... % 8 - 1 0 1 2 1 0 10 0 100 209 20 100 ]; % 9 + %1 0 1 2 0 0 100 0 100 209 20 100 ;... % 1 + %1 0 1 2 0 0 100 0 100 209 20 100 ;... % 2 + % 1 0 1 2 0 0 100 0 100 209 20 100 ;... % 3 + 1 0 1 2 0 1 100 0 100 209 20 20 ;... % 4 + 1 0 1 2 0 1 100 0 100 209 20 20 ;... % 5 + 1 0 1 2 0 0 100 0 100 209 20 20 ];%... % 6 + % 1 0 1 2 0 0 100 0 100 209 20 100 ;... % 7 + %1 0 1 2 0 0 100 0 100 209 20 1 ;... % 8 + %1 0 1 2 0 0 100 0 100 209 20 1 ]; % 9 noises = [ ... - % 1 2 3 4 5 6 7 - % sigma_ang sigma_cart sigma_accel sigma_gyro sigma_accelBias sigma_gyroBias sigma_gps - %1e-2 1e-1 1e-3 1e-5 0 0 1e-4;... % 1 - %1e-2 1e-1 1e-2 1e-5 0 0 1e-4;... % 2 - % 1e-2 1e-1 1e-1 1e-5 0 0 1e-4;... % 3 - %1e-2 1e-1 1e-3 1e-4 0 0 1e-4;... % 4 - %1e-2 1e-1 1e-3 1e-3 0 0 1e-4;... % 5 - %1e-2 1e-1 1e-3 1e-2 0 0 1e-4;... % 6 - % 1e-2 1e-1 1e-3 1e-1 0 0 1e-4;... % 7 - 1e-2 1e-1 1e-3 1e-5 1e-3 1e-5 1e-4;... % 8 - 1e-2 1e-1 1e-3 1e-5 1e-4 1e-6 1e-4]; % 9 + % 1 2 3 4 5 6 7 8 + % sigma_ang sigma_cart sigma_accel sigma_gyro sigma_accelBias sigma_gyroBias sigma_gps sigma_camera + %1e-2 1e-1 1e-3 1e-5 0 0 1e-4 1;... % 1 + %1e-2 1e-1 1e-2 1e-5 0 0 1e-4 1;... % 2 + % 1e-2 1e-1 1e-1 1e-5 0 0 1e-4 1;... % 3 + 1e-2 1e-1 1e-3 1e-4 0 0 1e-4 1;... % 4 + 1e-2 1e-1 1e-3 1e-3 0 0 1e-4 1;... % 5 + 1e-2 1e-1 1e-3 1e-2 0 0 1e-4 1];%... % 6 + % 1e-2 1e-1 1e-3 1e-1 0 0 1e-4 1;... % 7 + %1e-2 1e-1 1e-3 1e-2 1e-3 1e-5 1e-4 1;... % 8 + %1e-2 1e-1 1e-3 1e-2 1e-4 1e-6 1e-4 1]; % 9 if(size(testOptions,1) ~= size(noises,1)) error('testOptions and noises do not have same number of rows'); @@ -42,7 +42,7 @@ end externallyConfigured = 1; % Set the flag to save the results -saveResults = 1; +saveResults = 0; errorRuns = []; @@ -59,7 +59,7 @@ for i = 1:size(testOptions,1) options.imuFactorType = testOptions(i,4); options.imuNonzeroBias = testOptions(i,5); options.includeCameraFactors = testOptions(i,6); - numberOfLandmarks = testOptions(i,7); + options.numberOfLandmarks = testOptions(i,7); options.includeGPSFactors = testOptions(i,8); options.gpsStartPose = testOptions(i,9); options.trajectoryLength = testOptions(i,10); @@ -73,6 +73,7 @@ for i = 1:size(testOptions,1) sigma_accelBias = noises(i,5); sigma_gyroBias = noises(i,6); sigma_gps = noises(i,7); + sigma_camera = noises(i,8); % Create folder name f_between = ''; @@ -95,7 +96,7 @@ for i = 1:size(testOptions,1) f_between = sprintf('gps_%d_', gpsStartPose); end if (options.includeCameraFactors == 1) - f_camera = sprintf('camera_%d_', numberOfLandmarks); + f_camera = sprintf('camera_%d_', options.numberOfLandmarks); end f_runs = sprintf('mc%d', numMonteCarloRuns); From b83a9967318a2b31813e07b3450037d7946bc0e8 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Tue, 13 May 2014 17:04:16 -0400 Subject: [PATCH 73/90] Fixed SmartProjectionPoseFactor matlab wrapping --- gtsam_unstable/gtsam_unstable.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 440b26388..fdd75ae06 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -20,7 +20,6 @@ virtual class gtsam::NonlinearFactor; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; -virtual class gtsam::SharedNoiseModel; virtual class gtsam::Cal3_S2; class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; @@ -377,7 +376,7 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { void print(string s) const; }; -/* + #include template virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { @@ -386,9 +385,9 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); SmartProjectionPoseFactor(double rankTol); - //SmartProjectionPoseFactor(); + SmartProjectionPoseFactor(); - void add(const gtsam::Point2 measured_i, const gtsam::Key poseKey_i, const gtsam::SharedNoiseModel noise_i, + void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i); // enabling serialization functionality @@ -396,7 +395,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { }; typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; -*/ + #include template virtual class RangeFactor : gtsam::NonlinearFactor { From d0905e65ce5c73d9ebab438e1c265474d948ecae Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 15 May 2014 10:01:53 -0400 Subject: [PATCH 74/90] Working on smart projection factors for consistency tests --- .../+imuSimulator/covarianceAnalysisBetween.m | 28 +++++++++--- .../covarianceAnalysisCreateFactorGraph.m | 44 ++++++++++++++----- .../covarianceAnalysisCreateTrajectory.m | 22 +++++++--- 3 files changed, 71 insertions(+), 23 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index e6149b785..9f6648b8e 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -27,15 +27,15 @@ if ~exist('externallyConfigured', 'var') options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements options.includeCameraFactors = 1; % if true, SmartProjectionPose3Factors will be used with randomly generated landmarks - options.numberOfLandmarks = 100; % Total number of visual landmarks (randomly generated in a box around the trajectory) + options.numberOfLandmarks = 1000; % Total number of visual landmarks (randomly generated in a box around the trajectory) options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses options.gpsStartPose = 100; % Pose number to start including GPS factors at - options.trajectoryLength = 50;%209; % length of the ground truth trajectory + options.trajectoryLength = 20;%209; % length of the ground truth trajectory options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) - numMonteCarloRuns = 2; % number of Monte Carlo runs to perform + numMonteCarloRuns = 1; % number of Monte Carlo runs to perform % Noise values to be adjusted sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 @@ -88,10 +88,12 @@ noiseVectorGPS = sigma_gps * ones(3,1); noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]); %% Camera metadata -metadata.camera.calibration = Cal3_S2(500,500,0,640/2,480/2); % Camera calibration +metadata.camera.calibration = Cal3_S2(500,500,0,1920/2,1200/2); % Camera calibration metadata.camera.xlims = [-100, 650]; % x limits on area for landmark creation metadata.camera.ylims = [-100, 700]; % y limits on area for landmark creation -metadata.camera.zlims = [-10, 30]; % z limits on area for landmark creation +metadata.camera.zlims = [-30, 30]; % z limits on area for landmark creation +metadata.camera.visualRange = 100; % maximum distance from the camera that a landmark can be seen (meters) +metadata.camera.bodyPoseCamera = Pose3.Expmap([-pi/2;0;0;0;0;0]); % pose of camera in body metadata.camera.CameraSigma = sigma_camera; cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2, metadata.camera.CameraSigma); noiseVectorCamera = metadata.camera.CameraSigma .* ones(2,1); @@ -131,7 +133,7 @@ gtMeasurementNoise.gpsNoiseVector = zeros(3,1); metadata.imu.accelConstantBiasVector = zeros(3,1); metadata.imu.gyroConstantBiasVector = zeros(3,1); -gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... +[gtGraph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ... gtMeasurements, ... % ground truth measurements gtValues, ... % ground truth Values gtNoiseModels, ... % noise models to use in this graph @@ -145,6 +147,18 @@ gtGraph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... figure(1) hold on; +b = [-1000 2000 -2000 2000 -30 30]; +for i = 1:size(metadata.camera.gtLandmarkPoints,2) + p = metadata.camera.gtLandmarkPoints(i).vector; + if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6)) + plot3(p(1), p(2), p(3), 'k+'); + end +end +pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0)); +for i = 1:length(pointsToPlot) + p = pointsToPlot(i).vector; + plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10); +end plot3DPoints(gtValues); %plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); plot3DTrajectory(gtValues, '-r'); @@ -192,7 +206,7 @@ for k=1:numMonteCarloRuns end % Create a new graph using noisy measurements - graph = imuSimulator.covarianceAnalysisCreateFactorGraph( ... + [graph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ... gtMeasurements, ... % ground truth measurements gtValues, ... % ground truth Values monteCarloNoiseModels, ... % noise models to use in this graph diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 4ce15ced2..f7fd84989 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -1,4 +1,4 @@ -function [ graph ] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) +function [ graph projectionFactorSeenBy] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) % Create a factor graph based on provided measurements, values, and noises. % Used for covariance analysis scripts. % 'options' contains fields for including various factor types. @@ -36,9 +36,18 @@ for i=0:length(measurements) %% Create a SmartProjectionFactor for each landmark if options.includeCameraFactors == 1 for j=1:options.numberOfLandmarks - %% UNCOMMENT WHEN SMART FACTORS ARE ADDED TO MATLAB WRAPPER - %SmartProjectionFactors(j) = SmartProjectionPose3Factor(); + SmartProjectionFactors(j) = SmartProjectionPose3Factor(); + % Use constructor with default values, but express the pose of the + % camera as a 90 degree rotation about the X axis +% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ... +% 1, ... % rankTol +% -1, ... % linThreshold +% false, ... % manageDegeneracy +% false, ... % enableEPI +% metadata.camera.bodyPoseCamera); % Pose of camera in body frame + end + projectionFactorSeenBy = zeros(options.numberOfLandmarks,1); end else @@ -141,18 +150,21 @@ for i=0:length(measurements) end % end of IMU factor creation - %% Add Camera Factors + %% Build Camera Factors if options.includeCameraFactors == 1 - for(j = 1:length(measurements(i).landmarks)) + for j = 1:length(measurements(i).landmarks) cameraMeasurmentNoise = measurementNoise.cameraNoiseVector .* randn(2,1); cameraPixelMeasurement = measurements(i).landmarks(j).vector; - if(cameraPixelMeasurement(1) ~= 0 && cameraPixelMeasurement(2) ~= 0) + % Only add the measurement if it is in the image frame (based on calibration) + if(cameraPixelMeasurement(1) > 0 && cameraPixelMeasurement(2) > 0 ... + && cameraPixelMeasurement(1) < 2*metadata.camera.calibration.px ... + && cameraPixelMeasurement(2) < 2*metadata.camera.calibration.py) cameraPixelMeasurement = cameraPixelMeasurement + cameraMeasurmentNoise; - %% UNCOMMENT WHEN SMART FACTORS ARE ADDED TO MATLAB WRAPPER - % SmartProjectionFactors(j).add( ... - % Point2(cameraPizelMeasurement), ... - % currentPoseKey, noiseModels.noiseCamera, ... - % metadata.camera.calibration); + SmartProjectionFactors(j).add( ... + Point2(cameraPixelMeasurement), ... + currentPoseKey, noiseModels.noiseCamera, ... + metadata.camera.calibration); + projectionFactorSeenBy(j) = projectionFactorSeenBy(j)+1; end end end % end of Camera factor creation @@ -170,5 +182,15 @@ for i=0:length(measurements) end % end of for over trajectory +%% Add Camera Factors to the graph +[find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))] +if options.includeCameraFactors == 1 + for j = 1:options.numberOfLandmarks + if projectionFactorSeenBy(j) > 5 + graph.add(SmartProjectionFactors(j)); + end + end +end + end % end of function diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index be5991a31..8b55f85a6 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -29,6 +29,9 @@ if options.useRealData == 1 %% Generate the current pose currentPoseKey = symbol('x', i); currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd); + %% FOR TESTING + %currentPose = currentPose.compose(metadata.camera.bodyPoseCamera); + % add to values values.insert(currentPoseKey, currentPose); @@ -87,14 +90,23 @@ if options.useRealData == 1 end %% gt Camera measurements - if options.includeCameraFactors == 1 && i > 0 + if options.includeCameraFactors == 1 && i > 0 + % Create the camera based on the current pose and the pose of the + % camera in the body + cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera); + %camera = SimpleCamera(cameraPose, metadata.camera.calibration); camera = SimpleCamera(currentPose, metadata.camera.calibration); + + % Record measurements if the landmark is within visual range for j=1:length(metadata.camera.gtLandmarkPoints) - try - z = camera.project(metadata.camera.gtLandmarkPoints(j)); - measurements(i).landmarks(j) = z; - catch + distanceToLandmark = camera.pose.range(metadata.camera.gtLandmarkPoints(j)); + if distanceToLandmark < metadata.camera.visualRange + try + z = camera.project(metadata.camera.gtLandmarkPoints(j)); + measurements(i).landmarks(j) = z; + catch % point is probably out of the camera's view + end end end end From e5d4fe3aaf5164ddef3c310278164bc51b81e472 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 15 May 2014 13:21:35 -0400 Subject: [PATCH 75/90] replaced identity rotations with ground truth rotations --- .../covarianceAnalysisCreateFactorGraph.m | 7 +++++-- .../covarianceAnalysisCreateTrajectory.m | 2 +- .../+imuSimulator/getPoseFromGtScenario.m | 17 ++++++++++++++--- 3 files changed, 20 insertions(+), 6 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index f7fd84989..b6f79bf02 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -19,7 +19,8 @@ for i=0:length(measurements) if i==0 %% first time step, add priors % Pose prior (poses used for all factors) - initialPose = Pose3.Expmap(measurementNoise.poseNoiseVector .* randn(6,1)); + noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* rand(6,1); + initialPose = Pose3.Expmap(noisyInitialPoseVector); graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); % IMU velocity and bias priors @@ -183,10 +184,12 @@ for i=0:length(measurements) end % end of for over trajectory %% Add Camera Factors to the graph +% Only factors for landmarks that have been viewed at least once are added +% to the graph [find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))] if options.includeCameraFactors == 1 for j = 1:options.numberOfLandmarks - if projectionFactorSeenBy(j) > 5 + if projectionFactorSeenBy(j) > 0 graph.add(SmartProjectionFactors(j)); end end diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 8b55f85a6..eb735a5d7 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -30,7 +30,7 @@ if options.useRealData == 1 currentPoseKey = symbol('x', i); currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd); %% FOR TESTING - %currentPose = currentPose.compose(metadata.camera.bodyPoseCamera); + currentPose = currentPose.compose(metadata.camera.bodyPoseCamera); % add to values values.insert(currentPoseKey, currentPose); diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m index bf8da3343..070c0fb4c 100644 --- a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m +++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m @@ -18,8 +18,19 @@ dY = gtECEF(2) - initialPositionECEF(2); dZ = gtECEF(3) - initialPositionECEF(3); [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); -gtPosition = [xlt, ylt, zlt]'; -gtRotation = Rot3; % Rot3.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); -currentPose = Pose3(gtRotation, Point3(gtPosition)); +gtPosition = Point3([xlt, ylt, zlt]'); +% use the gtsam.Rot3.Ypr constructor (yaw, pitch, roll) from the ground truth data +% yaw = measured positively to the right +% pitch = measured positively up +% roll = measured positively to right +% Assumes vehice X forward, Y right, Z down +% +% In the gtScenario data +% heading (yaw) = measured positively to the left from Y-axis +% pitch = +% roll = +% Coordinate frame is Y forward, X is right, Z is up +gtBodyRotation = Rot3.Ypr(-gtScenario.Heading(scenarioInd), -gtScenario.Pitch(scenarioInd), -gtScenario.Roll(scenarioInd)); +currentPose = Pose3(gtBodyRotation, gtPosition); end \ No newline at end of file From 950e95b0156d75c3befe77b1004f9b30d38dfea1 Mon Sep 17 00:00:00 2001 From: djensen3 Date: Thu, 15 May 2014 15:57:22 -0400 Subject: [PATCH 76/90] Fixed error with IMU+Camera --- .../+imuSimulator/covarianceAnalysisBetween.m | 41 +++++++++++-------- .../covarianceAnalysisCreateFactorGraph.m | 8 ++-- .../covarianceAnalysisCreateTrajectory.m | 19 +++++---- .../+imuSimulator/getPoseFromGtScenario.m | 5 ++- 4 files changed, 42 insertions(+), 31 deletions(-) diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m index 9f6648b8e..2eddf75ee 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -20,22 +20,22 @@ if ~exist('externallyConfigured', 'var') %% Configuration % General options options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj - options.includeBetweenFactors = 1; % if true, BetweenFactors will be added between consecutive poses + options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses - options.includeIMUFactors = 0; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) + options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements options.includeCameraFactors = 1; % if true, SmartProjectionPose3Factors will be used with randomly generated landmarks - options.numberOfLandmarks = 1000; % Total number of visual landmarks (randomly generated in a box around the trajectory) + options.numberOfLandmarks = 1000; % Total number of visual landmarks (randomly generated in a box around the trajectory) options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses options.gpsStartPose = 100; % Pose number to start including GPS factors at - options.trajectoryLength = 20;%209; % length of the ground truth trajectory + options.trajectoryLength = 100;%209; % length of the ground truth trajectory options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) - numMonteCarloRuns = 1; % number of Monte Carlo runs to perform + numMonteCarloRuns = 2; % number of Monte Carlo runs to perform % Noise values to be adjusted sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 @@ -93,7 +93,7 @@ metadata.camera.xlims = [-100, 650]; % x limits on area for landmark creation metadata.camera.ylims = [-100, 700]; % y limits on area for landmark creation metadata.camera.zlims = [-30, 30]; % z limits on area for landmark creation metadata.camera.visualRange = 100; % maximum distance from the camera that a landmark can be seen (meters) -metadata.camera.bodyPoseCamera = Pose3.Expmap([-pi/2;0;0;0;0;0]); % pose of camera in body +metadata.camera.bodyPoseCamera = Pose3; % pose of camera in body metadata.camera.CameraSigma = sigma_camera; cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2, metadata.camera.CameraSigma); noiseVectorCamera = metadata.camera.CameraSigma .* ones(2,1); @@ -103,7 +103,7 @@ if options.includeCameraFactors == 1 for i = 1:options.numberOfLandmarks metadata.camera.gtLandmarkPoints(i) = Point3( ... [rand() * (metadata.camera.xlims(2)-metadata.camera.xlims(1)) + metadata.camera.xlims(1); ... - rand() * (metadata.camera.ylims(2)-metadata.camera.ylims(1)) + metadata.camera.ylims(1); + rand() * (metadata.camera.ylims(2)-metadata.camera.ylims(1)) + metadata.camera.ylims(1); ... rand() * (metadata.camera.zlims(2)-metadata.camera.zlims(1)) + metadata.camera.zlims(1)]); end end @@ -147,26 +147,31 @@ metadata.imu.gyroConstantBiasVector = zeros(3,1); figure(1) hold on; -b = [-1000 2000 -2000 2000 -30 30]; -for i = 1:size(metadata.camera.gtLandmarkPoints,2) - p = metadata.camera.gtLandmarkPoints(i).vector; - if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6)) - plot3(p(1), p(2), p(3), 'k+'); - end -end -pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0)); -for i = 1:length(pointsToPlot) - p = pointsToPlot(i).vector; - plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10); + +if options.includeCameraFactors + b = [-1000 2000 -2000 2000 -30 30]; + for i = 1:size(metadata.camera.gtLandmarkPoints,2) + p = metadata.camera.gtLandmarkPoints(i).vector; + if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6)) + plot3(p(1), p(2), p(3), 'k+'); + end + end + pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0)); + for i = 1:length(pointsToPlot) + p = pointsToPlot(i).vector; + plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10); + end end plot3DPoints(gtValues); %plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); plot3DTrajectory(gtValues, '-r'); + axis equal % optimize optimizer = GaussNewtonOptimizer(gtGraph, gtValues); gtEstimate = optimizer.optimize(); +plot3DTrajectory(gtEstimate, '-k'); % estimate should match gtValues if graph is correct. fprintf('Error in ground truth graph at gtValues: %g \n', gtGraph.error(gtValues) ); fprintf('Error in ground truth graph at gtEstimate: %g \n', gtGraph.error(gtEstimate) ); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index b6f79bf02..00ae4b9c2 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -19,7 +19,7 @@ for i=0:length(measurements) if i==0 %% first time step, add priors % Pose prior (poses used for all factors) - noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* rand(6,1); + noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* randn(6,1); initialPose = Pose3.Expmap(noisyInitialPoseVector); graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); @@ -35,9 +35,10 @@ for i=0:length(measurements) end %% Create a SmartProjectionFactor for each landmark + projectionFactorSeenBy = []; if options.includeCameraFactors == 1 for j=1:options.numberOfLandmarks - SmartProjectionFactors(j) = SmartProjectionPose3Factor(); + SmartProjectionFactors(j) = SmartProjectionPose3Factor(0.01); % Use constructor with default values, but express the pose of the % camera as a 90 degree rotation about the X axis % SmartProjectionFactors(j) = SmartProjectionPose3Factor( ... @@ -46,7 +47,6 @@ for i=0:length(measurements) % false, ... % manageDegeneracy % false, ... % enableEPI % metadata.camera.bodyPoseCamera); % Pose of camera in body frame - end projectionFactorSeenBy = zeros(options.numberOfLandmarks,1); end @@ -186,7 +186,7 @@ end % end of for over trajectory %% Add Camera Factors to the graph % Only factors for landmarks that have been viewed at least once are added % to the graph -[find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))] +%[find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))] if options.includeCameraFactors == 1 for j = 1:options.numberOfLandmarks if projectionFactorSeenBy(j) > 0 diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index eb735a5d7..2cceea753 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -9,7 +9,8 @@ import gtsam.*; values = Values; -warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data - currently using identity rotation') +warning('Rotating Pose inside getPoseFromGtScenario! TODO: Use a body_P_sensor transform in the factors') + if options.useRealData == 1 %% Create a ground truth trajectory from Real data (if available) @@ -29,8 +30,10 @@ if options.useRealData == 1 %% Generate the current pose currentPoseKey = symbol('x', i); currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd); - %% FOR TESTING - currentPose = currentPose.compose(metadata.camera.bodyPoseCamera); + + %% FOR TESTING - this is currently moved to getPoseFromGtScenario + %currentPose = currentPose.compose(metadata.camera.bodyPoseCamera); + %currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0])); % add to values values.insert(currentPoseKey, currentPose); @@ -57,7 +60,7 @@ if options.useRealData == 1 scenarioIndIMU2 = previousScenarioInd+j; IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1); IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2); - IMURot2 = IMUPose2.rotation.matrix; + IMURot1 = IMUPose1.rotation.matrix; IMUdeltaPose = IMUPose1.between(IMUPose2); IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose); @@ -71,10 +74,10 @@ if options.useRealData == 1 % deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; % acc = (deltaPosition - initialVel * dT) * (2/dt^2) - measurements(i).imu(j).accel = IMURot2' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT)); + measurements(i).imu(j).accel = IMURot1' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT)); % Update velocity - currentVel = currentVel + IMURot2 * measurements(i).imu(j).accel * deltaT; + currentVel = currentVel + IMURot1 * measurements(i).imu(j).accel * deltaT; end end @@ -94,8 +97,8 @@ if options.useRealData == 1 % Create the camera based on the current pose and the pose of the % camera in the body cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera); - %camera = SimpleCamera(cameraPose, metadata.camera.calibration); - camera = SimpleCamera(currentPose, metadata.camera.calibration); + camera = SimpleCamera(cameraPose, metadata.camera.calibration); + %camera = SimpleCamera(currentPose, metadata.camera.calibration); % Record measurements if the landmark is within visual range for j=1:length(metadata.camera.gtLandmarkPoints) diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m index 070c0fb4c..42dc1db60 100644 --- a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m +++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m @@ -30,7 +30,10 @@ gtPosition = Point3([xlt, ylt, zlt]'); % pitch = % roll = % Coordinate frame is Y forward, X is right, Z is up -gtBodyRotation = Rot3.Ypr(-gtScenario.Heading(scenarioInd), -gtScenario.Pitch(scenarioInd), -gtScenario.Roll(scenarioInd)); +gtBodyRotation = Rot3.Ypr(-gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); currentPose = Pose3(gtBodyRotation, gtPosition); +%% Rotate the pose +currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0])); + end \ No newline at end of file From a0c77dcc1ca8584a4a1b5c3fb4fd31076edbf92a Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Tue, 3 Jun 2014 23:52:35 -0400 Subject: [PATCH 77/90] remove unused variables in iterative solvers and rename accordingly. --- examples/Pose2SLAMwSPCG.cpp | 2 +- gtsam.h | 4 +--- gtsam/linear/IterativeSolver.cpp | 21 +++++++++----------- gtsam/linear/IterativeSolver.h | 16 ++++----------- gtsam/nonlinear/DoglegOptimizer.cpp | 2 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 2 +- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 12 +++++------ gtsam/nonlinear/NonlinearOptimizerParams.h | 6 +++--- 8 files changed, 26 insertions(+), 39 deletions(-) diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 2b2f63064..8e8e28570 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -104,7 +104,7 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT; + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; { parameters.iterativeParams = boost::make_shared(); diff --git a/gtsam.h b/gtsam.h index b7178d534..2cd6415a6 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1481,9 +1481,7 @@ class GaussianISAM { #include virtual class IterativeOptimizationParameters { - string getKernel() const ; string getVerbosity() const; - void setKernel(string s) ; void setVerbosity(string s) ; void print() const; }; @@ -1860,7 +1858,7 @@ virtual class NonlinearOptimizerParams { bool isMultifrontal() const; bool isSequential() const; bool isCholmod() const; - bool isCG() const; + bool isIterative() const; }; bool checkConvergence(double relativeErrorTreshold, diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 2bba3e12b..b022a26d8 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -11,18 +11,19 @@ namespace gtsam { -std::string IterativeOptimizationParameters::getKernel() const { return kernelTranslator(kernel_); } +/*****************************************************************************/ std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } -void IterativeOptimizationParameters::setKernel(const std::string &src) { kernel_ = kernelTranslator(src); } + +/*****************************************************************************/ void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); } -IterativeOptimizationParameters::Kernel IterativeOptimizationParameters::kernelTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "CG") return IterativeOptimizationParameters::CG; - /* default is cg */ - else return IterativeOptimizationParameters::CG; +/*****************************************************************************/ +void IterativeOptimizationParameters::print() const { + std::cout << "IterativeOptimizationParameters" << std::endl + << "verbosity: " << verbosityTranslator(verbosity_) << std::endl; } +/*****************************************************************************/ IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src) { std::string s = src; boost::algorithm::to_upper(s); if (s == "SILENT") return IterativeOptimizationParameters::SILENT; @@ -32,11 +33,7 @@ IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verb else return IterativeOptimizationParameters::SILENT; } -std::string IterativeOptimizationParameters::kernelTranslator(IterativeOptimizationParameters::Kernel k) { - if ( k == CG ) return "CG"; - else return "UNKNOWN"; -} - +/*****************************************************************************/ std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { if (verbosity == SILENT) return "SILENT"; else if (verbosity == COMPLEXITY) return "COMPLEXITY"; diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 988293a4f..b4e2e6d49 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -30,21 +30,19 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - enum Kernel { CG = 0 } kernel_ ; ///< Iterative Method Kernel enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_; public: IterativeOptimizationParameters(const IterativeOptimizationParameters &p) - : kernel_(p.kernel_), verbosity_(p.verbosity_) {} + : verbosity_(p.verbosity_) {} - IterativeOptimizationParameters(const Kernel kernel = CG, const Verbosity verbosity = SILENT) - : kernel_(kernel), verbosity_(verbosity) {} + IterativeOptimizationParameters(const Verbosity verbosity = SILENT) + : verbosity_(verbosity) {} virtual ~IterativeOptimizationParameters() {} /* general interface */ - inline Kernel kernel() const { return kernel_; } inline Verbosity verbosity() const { return verbosity_; } /* matlab interface */ @@ -53,15 +51,9 @@ namespace gtsam { void setKernel(const std::string &s) ; void setVerbosity(const std::string &s) ; - virtual void print() const { - std::cout << "IterativeOptimizationParameters" << std::endl - << "kernel: " << kernelTranslator(kernel_) << std::endl - << "verbosity: " << verbosityTranslator(verbosity_) << std::endl; - } + virtual void print() const ; - static Kernel kernelTranslator(const std::string &s); static Verbosity verbosityTranslator(const std::string &s); - static std::string kernelTranslator(Kernel k); static std::string verbosityTranslator(Verbosity v); }; diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 03da76eb6..082cc66c8 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -76,7 +76,7 @@ void DoglegOptimizer::iterate(void) { result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, dx_u, dx_n, bn, graph_, state_.values, state_.error, dlVerbose); } - else if ( params_.isCG() ) { + else if ( params_.isIterative() ) { throw runtime_error("Dogleg is not currently compatible with the linear conjugate gradient solver"); } else { diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 54e5cb9e3..678cac6b4 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -102,7 +102,7 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, } else if (params.isSequential()) { delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); - } else if (params.isCG()) { + } else if (params.isIterative()) { if (!params.iterativeParams) throw std::runtime_error( "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 7b0e73985..e0b6e0c6c 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -99,8 +99,8 @@ void NonlinearOptimizerParams::print(const std::string& str) const { case CHOLMOD: std::cout << " linear solver type: CHOLMOD\n"; break; - case CONJUGATE_GRADIENT: - std::cout << " linear solver type: CONJUGATE GRADIENT\n"; + case Iterative: + std::cout << " linear solver type: ITERATIVE\n"; break; default: std::cout << " linear solver type: (invalid)\n"; @@ -127,8 +127,8 @@ std::string NonlinearOptimizerParams::linearSolverTranslator( return "SEQUENTIAL_CHOLESKY"; case SEQUENTIAL_QR: return "SEQUENTIAL_QR"; - case CONJUGATE_GRADIENT: - return "CONJUGATE_GRADIENT"; + case Iterative: + return "ITERATIVE"; case CHOLMOD: return "CHOLMOD"; default: @@ -148,8 +148,8 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve return SEQUENTIAL_CHOLESKY; if (linearSolverType == "SEQUENTIAL_QR") return SEQUENTIAL_QR; - if (linearSolverType == "CONJUGATE_GRADIENT") - return CONJUGATE_GRADIENT; + if (linearSolverType == "ITERATIVE") + return Iterative; if (linearSolverType == "CHOLMOD") return CHOLMOD; throw std::invalid_argument( diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 10d468384..641c6da24 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -99,7 +99,7 @@ public: MULTIFRONTAL_QR, SEQUENTIAL_CHOLESKY, SEQUENTIAL_QR, - CONJUGATE_GRADIENT, /* Experimental Flag */ + Iterative, /* Experimental Flag */ CHOLMOD, /* Experimental Flag */ }; @@ -121,8 +121,8 @@ public: return (linearSolverType == CHOLMOD); } - inline bool isCG() const { - return (linearSolverType == CONJUGATE_GRADIENT); + inline bool isIterative() const { + return (linearSolverType == Iterative); } GaussianFactorGraph::Eliminate getEliminationFunction() const { From c844ab19e3e7dd6665fb98dcbd834d3cb70b92e8 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Wed, 4 Jun 2014 23:23:17 -0400 Subject: [PATCH 78/90] move blas_kernel_ up --- gtsam/linear/ConjugateGradientSolver.cpp | 47 ++++++++++++++++++++++++ gtsam/linear/ConjugateGradientSolver.h | 33 ++++++++++------- gtsam/linear/IterativeSolver.h | 6 --- 3 files changed, 66 insertions(+), 20 deletions(-) create mode 100644 gtsam/linear/ConjugateGradientSolver.cpp diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp new file mode 100644 index 000000000..4072c5080 --- /dev/null +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -0,0 +1,47 @@ +/* + * ConjugateGradientSolver.cpp + * + * Created on: Jun 4, 2014 + * Author: ydjian + */ + +#include +#include + +namespace gtsam { + +/*****************************************************************************/ +std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) { + std::string s; + switch (value) { + case ConjugateGradientParameters::GTSAM: s = "GTSAM" ; break; + default: s = "UNDEFINED" ; break; + } + return s; +} + +/*****************************************************************************/ +ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "GTSAM") return ConjugateGradientParameters::GTSAM; + + /* default is SBM */ + return ConjugateGradientParameters::GTSAM; +} + +/*****************************************************************************/ + +/*****************************************************************************/ +void ConjugateGradientParameters::print() const { + Base::print(); + std::cout << "ConjugateGradientParameters" << std::endl + << "minIter: " << minIterations_ << std::endl + << "maxIter: " << maxIterations_ << std::endl + << "resetIter: " << reset_ << std::endl + << "eps_rel: " << epsilon_rel_ << std::endl + << "eps_abs: " << epsilon_abs_ << std::endl; + } + +} + + diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index d1b3b2c7e..ce27b7a96 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -20,6 +20,7 @@ namespace gtsam { */ class ConjugateGradientParameters : public IterativeOptimizationParameters { + public: typedef IterativeOptimizationParameters Base; typedef boost::shared_ptr shared_ptr; @@ -30,14 +31,23 @@ public: double epsilon_rel_; ///< threshold for relative error decrease double epsilon_abs_; ///< threshold for absolute error decrease - ConjugateGradientParameters() - : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), epsilon_abs_(1e-3){} + /* Matrix Operation Kernel */ + enum BLASKernel { + GTSAM = 0, ///< Jacobian Factor Graph of GTSAM + } blas_kernel_ ; - ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset, double epsilon_rel, double epsilon_abs) - : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs){} + ConjugateGradientParameters() + : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), + epsilon_abs_(1e-3), blas_kernel_(GTSAM) {} + + ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset, + double epsilon_rel, double epsilon_abs, BLASKernel blas) + : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), + epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {} ConjugateGradientParameters(const ConjugateGradientParameters &p) - : Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_), epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_) {} + : Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_), + epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_(GTSAM) {} /* general interface */ inline size_t minIterations() const { return minIterations_; } @@ -61,15 +71,10 @@ public: inline void setEpsilon_rel(double value) { epsilon_rel_ = value; } inline void setEpsilon_abs(double value) { epsilon_abs_ = value; } - virtual void print() const { - Base::print(); - std::cout << "ConjugateGradientParameters" << std::endl - << "minIter: " << minIterations_ << std::endl - << "maxIter: " << maxIterations_ << std::endl - << "resetIter: " << reset_ << std::endl - << "eps_rel: " << epsilon_rel_ << std::endl - << "eps_abs: " << epsilon_abs_ << std::endl; - } + static std::string blasTranslator(const BLASKernel k) ; + static BLASKernel blasTranslator(const std::string &s) ; + + virtual void print() const; }; } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index b4e2e6d49..daf5960e7 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -14,7 +14,6 @@ #include #include -#include namespace gtsam { @@ -46,9 +45,7 @@ namespace gtsam { inline Verbosity verbosity() const { return verbosity_; } /* matlab interface */ - std::string getKernel() const ; std::string getVerbosity() const; - void setKernel(const std::string &s) ; void setVerbosity(const std::string &s) ; virtual void print() const ; @@ -68,9 +65,6 @@ namespace gtsam { /* interface to the nonlinear optimizer */ virtual VectorValues optimize (const VectorValues &initial) = 0; - - /* update interface to the nonlinear optimizer */ - virtual void replaceFactors(const boost::shared_ptr &factorGraph, const double lambda) {} }; } From e8d38099174b8d7604c95ff7260ac48220381bb1 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Sun, 8 Jun 2014 00:34:23 -0400 Subject: [PATCH 79/90] add new generic pcgsolver and preconditioner classes add a unit test for the PCGSolver class --- gtsam/inference/Ordering.h | 11 + gtsam/linear/ConjugateGradientSolver.cpp | 26 +- gtsam/linear/ConjugateGradientSolver.h | 75 +++- gtsam/linear/GaussianFactorGraph.cpp | 14 + gtsam/linear/GaussianFactorGraph.h | 3 + gtsam/linear/IterativeSolver.cpp | 101 +++++- gtsam/linear/IterativeSolver.h | 101 +++++- gtsam/linear/PCGSolver.cpp | 437 +++++++++++++++++++++++ gtsam/linear/PCGSolver.h | 236 ++++++++++++ gtsam/linear/Preconditioner.cpp | 212 +++++++++++ gtsam/linear/Preconditioner.h | 173 +++++++++ gtsam/linear/SubgraphSolver.h | 17 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 22 +- tests/testPCGSolver.cpp | 77 ++++ 14 files changed, 1453 insertions(+), 52 deletions(-) create mode 100644 gtsam/linear/PCGSolver.cpp create mode 100644 gtsam/linear/PCGSolver.h create mode 100644 gtsam/linear/Preconditioner.cpp create mode 100644 gtsam/linear/Preconditioner.h create mode 100644 tests/testPCGSolver.cpp diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index ea2ad7eda..7b1a2bb2e 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -17,9 +17,11 @@ #pragma once +#include #include #include +#include #include #include #include @@ -135,6 +137,15 @@ namespace gtsam { static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex, const FastMap& groups); + /// Return a natural Ordering. Typically used by iterative solvers + template + static Ordering Natural(const FactorGraph &fg) { + FastSet src = fg.keys(); + std::vector keys(src.begin(), src.end()); + std::stable_sort(keys.begin(), keys.end()); + return Ordering(keys); + } + /// @} /// @name Testable @{ diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 4072c5080..0505f6c01 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -6,10 +6,24 @@ */ #include +#include #include +using namespace std; + namespace gtsam { +/*****************************************************************************/ +void ConjugateGradientParameters::print(ostream &os) const { + Base::print(os); + cout << "ConjugateGradientParameters" << endl + << "minIter: " << minIterations_ << endl + << "maxIter: " << maxIterations_ << endl + << "resetIter: " << reset_ << endl + << "eps_rel: " << epsilon_rel_ << endl + << "eps_abs: " << epsilon_abs_ << endl; +} + /*****************************************************************************/ std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) { std::string s; @@ -29,18 +43,6 @@ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTransla return ConjugateGradientParameters::GTSAM; } -/*****************************************************************************/ - -/*****************************************************************************/ -void ConjugateGradientParameters::print() const { - Base::print(); - std::cout << "ConjugateGradientParameters" << std::endl - << "minIter: " << minIterations_ << std::endl - << "maxIter: " << maxIterations_ << std::endl - << "resetIter: " << reset_ << std::endl - << "eps_rel: " << epsilon_rel_ << std::endl - << "eps_abs: " << epsilon_abs_ << std::endl; - } } diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index ce27b7a96..47ec4b405 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -12,13 +12,13 @@ #pragma once #include +#include namespace gtsam { /** * parameters for the conjugate gradient method */ - class ConjugateGradientParameters : public IterativeOptimizationParameters { public: @@ -71,10 +71,79 @@ public: inline void setEpsilon_rel(double value) { epsilon_rel_ = value; } inline void setEpsilon_abs(double value) { epsilon_abs_ = value; } + virtual void print(std::ostream &os) const; + static std::string blasTranslator(const BLASKernel k) ; static BLASKernel blasTranslator(const std::string &s) ; - - virtual void print() const; }; +/*********************************************************************************************/ +/* + * A template of linear preconditioned conjugate gradient method. + * System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v, + * rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y) + * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. + */ +template +V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { + + V estimate, residual, direction, q1, q2; + estimate = residual = direction = q1 = q2 = initial; + + system.residual(estimate, q1); /* q1 = b-Ax */ + system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ + system.rightPrecondition(residual, direction);/* d = S^{-1} r */ + + double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta; + + const size_t iMaxIterations = parameters.maxIterations(), + iMinIterations = parameters.minIterations(), + iReset = parameters.reset() ; + const double threshold = std::max(parameters.epsilon_abs(), + parameters.epsilon() * parameters.epsilon() * currentGamma); + + if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY ) + std::cout << "[PCG] epsilon = " << parameters.epsilon() + << ", max = " << parameters.maxIterations() + << ", reset = " << parameters.reset() + << ", ||r0||^2 = " << currentGamma + << ", threshold = " << threshold << std::endl; + + size_t k; + for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= iMinIterations) ; k++ ) { + + if ( k % iReset == 0 ) { + system.residual(estimate, q1); /* q1 = b-Ax */ + system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ + system.rightPrecondition(residual, direction); /* d = S^{-1} r */ + currentGamma = system.dot(residual, residual); + } + system.multiply(direction, q1); /* q1 = A d */ + alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */ + system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */ + system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */ + system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */ + prevGamma = currentGamma; + currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */ + beta = currentGamma / prevGamma; + system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */ + system.scal(beta, direction); + system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */ + + if (parameters.verbosity() >= ConjugateGradientParameters::ERROR ) + std::cout << "[PCG] k = " << k + << ", alpha = " << alpha + << ", beta = " << beta + << ", ||r||^2 = " << currentGamma + << std::endl; + } + if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY ) + std::cout << "[PCG] iterations = " << k + << ", ||r||^2 = " << currentGamma + << std::endl; + + return estimate; +} + + } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index d6836783b..f10309d76 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -54,6 +54,20 @@ namespace gtsam { return keys; } + /* ************************************************************************* */ + std::map GaussianFactorGraph::getKeyDimMap() const { + map spec; + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, *this ) { + for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) { + map::iterator it2 = spec.find(*it); + if ( it2 == spec.end() ) { + spec.insert(make_pair(*it, gf->getDim(it))); + } + } + } + return spec; + } + /* ************************************************************************* */ vector GaussianFactorGraph::getkeydim() const { // First find dimensions of each variable diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 692310f26..910b25d1e 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -138,6 +138,9 @@ namespace gtsam { typedef FastSet Keys; Keys keys() const; + /* return a map of (Key, dimension) */ + std::map getKeyDimMap() const; + std::vector getkeydim() const; /** unnormalized error */ diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index b022a26d8..36178b191 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -6,26 +6,42 @@ */ #include +#include +#include #include +#include #include +using namespace std; + namespace gtsam { /*****************************************************************************/ -std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } +string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } /*****************************************************************************/ -void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); } +void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); } /*****************************************************************************/ void IterativeOptimizationParameters::print() const { - std::cout << "IterativeOptimizationParameters" << std::endl - << "verbosity: " << verbosityTranslator(verbosity_) << std::endl; + print(cout); } /*****************************************************************************/ -IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); +void IterativeOptimizationParameters::print(ostream &os) const { + os << "IterativeOptimizationParameters:" << endl + << "verbosity: " << verbosityTranslator(verbosity_) << endl; +} + +/*****************************************************************************/ + ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { + p.print(os); + return os; +} + +/*****************************************************************************/ +IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) { + string s = src; boost::algorithm::to_upper(s); if (s == "SILENT") return IterativeOptimizationParameters::SILENT; else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; @@ -34,13 +50,84 @@ IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verb } /*****************************************************************************/ -std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { + string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { if (verbosity == SILENT) return "SILENT"; else if (verbosity == COMPLEXITY) return "COMPLEXITY"; else if (verbosity == ERROR) return "ERROR"; else return "UNKNOWN"; } +/*****************************************************************************/ +VectorValues IterativeSolver::optimize( + const GaussianFactorGraph &gfg, + boost::optional keyInfo, + boost::optional&> lambda) +{ + return optimize( + gfg, + keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); +} + +/*****************************************************************************/ +VectorValues IterativeSolver::optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda) +{ + return optimize(gfg, keyInfo, lambda, keyInfo.x0()); +} + +/****************************************************************************/ +KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) + : ordering_(ordering) { + initialize(fg); +} + +/****************************************************************************/ +KeyInfo::KeyInfo(const GaussianFactorGraph &fg) + : ordering_(Ordering::Natural(fg)) { + initialize(fg); +} + +/****************************************************************************/ +void KeyInfo::initialize(const GaussianFactorGraph &fg){ + const map colspec = fg.getKeyDimMap(); + const size_t n = ordering_.size(); + size_t start = 0; + + for ( size_t i = 0 ; i < n ; ++i ) { + const size_t key = ordering_[i]; + const size_t dim = colspec.find(key)->second; + insert(make_pair(key, KeyInfoEntry(i, dim, start))); + start += dim ; + } + numCols_ = start; +} + +/****************************************************************************/ +vector KeyInfo::colSpec() const { + std::vector result(size(), 0); + BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + result[item.second.index()] = item.second.dim(); + } + return result; +} + +/****************************************************************************/ +VectorValues KeyInfo::x0() const { + VectorValues result; + BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + result.insert(item.first, Vector::Zero(item.second.dim())); + } + return result; +} + +/****************************************************************************/ +Vector KeyInfo::x0vector() const { + return Vector::Zero(numCols_); +} + } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index daf5960e7..66006875b 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -11,16 +11,28 @@ #pragma once +#include #include - +#include +#include +#include +#include +#include +#include #include +#include namespace gtsam { // Forward declarations - class VectorValues; + class KeyInfo; + class KeyInfoEntry; + class Ordering; class GaussianFactorGraph; + class Values; + class VectorValues; + /************************************************************************************/ /** * parameters for iterative linear solvers */ @@ -33,38 +45,97 @@ namespace gtsam { public: - IterativeOptimizationParameters(const IterativeOptimizationParameters &p) - : verbosity_(p.verbosity_) {} - - IterativeOptimizationParameters(const Verbosity verbosity = SILENT) - : verbosity_(verbosity) {} + IterativeOptimizationParameters(Verbosity v = SILENT) + : verbosity_(v) {} virtual ~IterativeOptimizationParameters() {} - /* general interface */ + /* utility */ inline Verbosity verbosity() const { return verbosity_; } - - /* matlab interface */ std::string getVerbosity() const; void setVerbosity(const std::string &s) ; - virtual void print() const ; + /* matlab interface */ + void print() const ; + + /* virtual print function */ + virtual void print(std::ostream &os) const ; + + /* for serialization */ + friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p); static Verbosity verbosityTranslator(const std::string &s); static std::string verbosityTranslator(Verbosity v); }; + /************************************************************************************/ class GTSAM_EXPORT IterativeSolver { public: typedef boost::shared_ptr shared_ptr; IterativeSolver() {} virtual ~IterativeSolver() {} - /* interface to the nonlinear optimizer */ - virtual VectorValues optimize () = 0; + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + VectorValues optimize ( + const GaussianFactorGraph &gfg, + boost::optional = boost::none, + boost::optional&> lambda = boost::none + ); + + /* interface to the nonlinear optimizer, without initial estimate */ + VectorValues optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda + ); + + /* interface to the nonlinear optimizer that the subclasses have to implement */ + virtual VectorValues optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial + ) = 0; - /* interface to the nonlinear optimizer */ - virtual VectorValues optimize (const VectorValues &initial) = 0; }; + /************************************************************************************/ + /* Handy data structure for iterative solvers + * key to (index, dimension, colstart) */ + class KeyInfoEntry : public boost::tuple { + public: + typedef boost::tuple Base; + KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} + const size_t index() const { return this->get<0>(); } + const size_t dim() const { return this->get<1>(); } + const size_t colstart() const { return this->get<2>(); } + }; + + /************************************************************************************/ + class KeyInfo : public std::map { + public: + typedef std::map Base; + KeyInfo(const GaussianFactorGraph &fg); + KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); + + std::vector colSpec() const ; + VectorValues x0() const; + Vector x0vector() const; + + inline size_t numCols() const { return numCols_; } + inline const Ordering & ordering() const { return ordering_; } + + protected: + + void initialize(const GaussianFactorGraph &fg); + + Ordering ordering_; + size_t numCols_; + + private: + KeyInfo() {} + + }; + + } diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp new file mode 100644 index 000000000..6bfa389fb --- /dev/null +++ b/gtsam/linear/PCGSolver.cpp @@ -0,0 +1,437 @@ +/* + * PCGSolver.cpp + * + * Created on: Feb 14, 2012 + * Author: ydjian + */ + +#include +//#include +//#include +//#include +//#include +#include +#include +//#include +//#include +//#include +//#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/*****************************************************************************/ +void PCGSolverParameters::print(ostream &os) const { + Base::print(os); + os << "PCGSolverParameters:" << endl; + preconditioner_->print(os); +} + +/*****************************************************************************/ +PCGSolver::PCGSolver(const PCGSolverParameters &p) { + preconditioner_ = createPreconditioner(p.preconditioner_); +} + +/*****************************************************************************/ +VectorValues PCGSolver::optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial) +{ + /* build preconditioner */ + preconditioner_->build(gfg, keyInfo, lambda); + + /* apply pcg */ + const Vector sol = preconditionedConjugateGradient( + GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda), + initial.vector(keyInfo.ordering()), parameters_); + + return buildVectorValues(sol, keyInfo); +} + +/*****************************************************************************/ +GaussianFactorGraphSystem::GaussianFactorGraphSystem( + const GaussianFactorGraph &gfg, + const Preconditioner &preconditioner, + const KeyInfo &keyInfo, + const std::map &lambda) + : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {} + +/*****************************************************************************/ +void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { + /* implement b-Ax, assume x and r are pre-allocated */ + + /* reset r to b */ + getb(r); + + /* substract A*x */ + Vector Ax = Vector::Zero(r.rows(), 1); + multiply(x, Ax); + r -= Ax ; +} + +/*****************************************************************************/ +void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const { + /* implement Ax, assume x and Ax are pre-allocated */ + + /* reset y */ + Ax.setZero(); + + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + /* accumulate At A x*/ + for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { + const Matrix Ai = jf->getA(it); + /* this map lookup should be replaced */ + const KeyInfoEntry &entry = keyInfo_.find(*it)->second; + Ax.segment(entry.colstart(), entry.dim()) + += Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim())); + } + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + /* accumulate H x */ + + /* use buffer to avoid excessive table lookups */ + const size_t sz = hf->size(); + vector y; + y.reserve(sz); + for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { + /* initialize y to zeros */ + y.push_back(zero(hf->getDim(it))); + } + + for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) { + /* retrieve the key mapping */ + const KeyInfoEntry &entry = keyInfo_.find(*j)->second; + // xj is the input vector + const Vector xj = x.segment(entry.colstart(), entry.dim()); + size_t idx = 0; + for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) { + if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj; + else y[idx] += hf->info(i, j).knownOffDiagonal() * xj; + } + } + + /* accumulate to r */ + for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) { + /* retrieve the key mapping */ + const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second; + Ax.segment(entry.colstart(), entry.dim()) += y[i]; + } + } + else { + throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); + } + } +} + +/*****************************************************************************/ +void GaussianFactorGraphSystem::getb(Vector &b) const { + /* compute rhs, assume b pre-allocated */ + + /* reset */ + b.setZero(); + + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + const Vector rhs = jf->getb(); + /* accumulate At rhs */ + for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { + /* this map lookup should be replaced */ + const KeyInfoEntry &entry = keyInfo_.find(*it)->second; + b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ; + } + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + /* accumulate g */ + for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { + const KeyInfoEntry &entry = keyInfo_.find(*it)->second; + b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it); + } + } + else { + throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); + } + } +} + +/**********************************************************************************/ +void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const +{ preconditioner_.transposeSolve(x, y); } + +/**********************************************************************************/ +void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const +{ preconditioner_.solve(x, y); } + +/**********************************************************************************/ +VectorValues buildVectorValues(const Vector &v, + const Ordering &ordering, + const map & dimensions) { + VectorValues result; + + DenseIndex offset = 0; + for ( size_t i = 0 ; i < ordering.size() ; ++i ) { + const Key &key = ordering[i]; + map::const_iterator it = dimensions.find(key); + if ( it == dimensions.end() ) { + throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions"); + } + const size_t dim = it->second; + result.insert(key, v.segment(offset, dim)); + offset += dim; + } + + return result; +} + +/**********************************************************************************/ +VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { + VectorValues result; + BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { + result.insert(item.first, v.segment(item.second.colstart(), item.second.dim())); + } + return result; +} + +///*****************************************************************************/ +//std::string PCGSolverParameters::pcgTranslator(const PCGKernel value) { +// std::string s; +// switch (value) { +// case PCGSolverParameters::PCG: s = "PCG"; break; +// case PCGSolverParameters::LSPCG: s = "LSPCG"; break; +// case PCGSolverParameters::SPCG: s = "SPCG"; break; +// default: s = "UNDEFINED"; break; +// } +// return s; +//} +// +///*****************************************************************************/ +//PCGSolverParameters::PCGKernel PCGSolverParameters::pcgTranslator(const std::string &src) { +// std::string s = src; boost::algorithm::to_upper(s); +// if (s == "PCG") return PCGSolverParameters::PCG; +// if (s == "LSPCG") return PCGSolverParameters::LSPCG; +// if (s == "SPCG") return PCGSolverParameters::SPCG; +// +// /* default is LSPCG */ +// return PCGSolverParameters::LSPCG; +//} +// +///*****************************************************************************/ +//std::string PCGSolverParameters::blasTranslator(const BLASKernel value) { +// std::string s; +// switch (value) { +// case PCGSolverParameters::GTSAM: s = "GTSAM" ; break; +// case PCGSolverParameters::SBM: s = "SBM" ; break; +// case PCGSolverParameters::SBM_MT: s = "SBM-MT" ; break; +// default: s = "UNDEFINED" ; break; +// } +// return s; +//} +// +///*****************************************************************************/ +//PCGSolverParameters::BLASKernel PCGSolverParameters::blasTranslator(const std::string &src) { +// std::string s = src; boost::algorithm::to_upper(s); +// if (s == "GTSAM") return PCGSolverParameters::GTSAM; +// if (s == "SBM") return PCGSolverParameters::SBM; +// if (s == "SBM-MT") return PCGSolverParameters::SBM_MT; +// +// /* default is SBM */ +// return PCGSolverParameters::SBM; +//} +// +///*****************************************************************************/ +//std::string PCGSolverParameters::rbTranslator(const RegisterBlockKernel k) { +// std::string s; +// switch (k) { +// case PCGSolverParameters::RB_NONE: s = "RB_NONE" ; break; +// case PCGSolverParameters::SSE_RB22: s = "SSE_RB22" ; break; +// case PCGSolverParameters::SSE_RB44: s = "SSE_RB44" ; break; +// case PCGSolverParameters::AVX_RB44: s = "AVX_RB44" ; break; +// default: s = "UNDEFINED" ; break; +// } +// return s; +//} +// +///*****************************************************************************/ +//PCGSolverParameters::RegisterBlockKernel PCGSolverParameters::rbTranslator(const std::string &src) { +// std::string s = src; boost::algorithm::to_upper(s); +// if (s == "") return PCGSolverParameters::RB_NONE; +// if (s == "SSE_RB22") return PCGSolverParameters::SSE_RB22; +// if (s == "SSE_RB44") return PCGSolverParameters::SSE_RB44; +// if (s == "AVX_RB44") return PCGSolverParameters::AVX_RB44; +// +// /* default is SBM */ +// return PCGSolverParameters::RB_NONE; +//} +// +///*****************************************************************************/ +//PCGSolver::PCGSolver(const PCGSolverParameters &p) : Base(), parameters_(p), built_(false) { +// preconditioner_ = createPreconditioner(p.preconditioner_); +//} +// +///*****************************************************************************/ +//void PCGSolver::replaceFactors( +// const Values &linearization_point, +// const GaussianFactorGraph &gfg, +// const double lambda) +//{ +// const JacobianFactorGraph jfg(gfg); +// +// /* prepare the column structure */ +// if ( keyInfo_.size() == 0 ) { +// keyInfo_ = KeyInfo(jfg, *orderingNatural(jfg)); +// } +// +// /* implemented by subclass */ +// safe_tic_(); +// convertKernel(jfg, lambda); +// safe_toc_("convert-kernel"); +// +// /* update the preconditioner as well */ +// preconditioner_->replaceFactors(jfg, lambda); +// built_ = false; +//} +// +///*****************************************************************************/ +//void PCGSolver::buildPreconditioner() { +// if ( built_ == false ) { +// safe_tic_(); +// preconditioner_->buildPreconditioner(); +// built_ = true; +// safe_toc_("factorize"); +// } +//} +// +///*****************************************************************************/ +//VectorValues PCGSolver::optimize() { +// +// buildPreconditioner(); +// +// VectorValues zero; +// BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo_ ) { +// zero.insert(item.first, Vector::Zero(item.second.dim())); +// } +// +// return optimize(zero); +//} +// +///*****************************************************************************/ +//VectorValues PCGSolver::optimize(const VectorValues &initial) { +// safe_tic_(); +// VectorValues result = optimize_(initial); +// safe_toc_("pcg"); +// return result ; +//} +// +///*****************************************************************************/ +////void PCGSolver_FG::convertKernel(const JacobianFactorGraph &jfg, const double lambda) { +//// hfg_ = buildOuterProductJacobianFactorGraph(jfg, VariableIndex(jfg), *x0_, lambda); +////} +//// +////VectorValues PCGSolver_FG::optimize_(const VectorValues &initial) { +//// System system(hfg_, preconditioner_); +//// return preconditionedConjugateGradient(system, initial, parameters_); +////} +// +///*****************************************************************************/ +//void PCGSolver_SBM::convertKernel(const JacobianFactorGraph &jfg, const double lambda) { +// linear_ = buildSparseLinearSystem(jfg, keyInfo_, true /* AtA */, lambda, false /* column major */, +// parameters_.blas_kernel_, parameters_.rb_kernel_); +//} +// +///*****************************************************************************/ +//VectorValues PCGSolver_SBM::optimize_(const VectorValues &initial) { +// System system(linear_, preconditioner_); +// Vector solution = preconditionedConjugateGradient( +// system, initial.vector(keyInfo_.ordering()), parameters_); +// return buildVectorValues(solution, keyInfo_); +//} +// +///*****************************************************************************/ +//ydjian::SparseLinearSystem::shared_ptr +//buildSparseLinearSystem(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, +// const bool AtA, const double lambda, const bool colMajor, +// const PCGSolverParameters::BLASKernel blas, const PCGSolverParameters::RegisterBlockKernel rbSrc) { +// +// std::map rbMap; +// rbMap[PCGSolverParameters::RB_NONE] = ydjian::SBM::NAIVE; +// rbMap[PCGSolverParameters::SSE_RB22] = ydjian::SBM::SSE_RB22; +// rbMap[PCGSolverParameters::SSE_RB44] = ydjian::SBM::SSE_RB44; +// rbMap[PCGSolverParameters::AVX_RB44] = ydjian::SBM::AVX_RB44; +// +// ydjian::SBM::Type rb; +// SC_ASSERT( rbMap.find(rbSrc) != rbMap.end(), "buildSparseLinearSystem: rbSrc is not supported.."); +// rb = rbMap[rbSrc]; +// +// ydjian::SparseLinearSystem::shared_ptr linear; +// +// switch (blas) { +// +// case PCGSolverParameters::SBM: +// linear = gsp2::buildSparseLinearSystem(jfg, keyInfo, AtA, lambda, colMajor, false /* multithread */, rb); +// break; +// +// case PCGSolverParameters::SBM_MT: +// linear = gsp2::buildSparseLinearSystem(jfg, keyInfo, AtA, lambda, colMajor, true /* multithread */, rb); +// break; +// +// default: +// throw std::invalid_argument("createSparseLinearSystem: unsupported blas kernel"); +// break; +// } +// +// return linear; +//} +// +///*****************************************************************************/ +//PCGSolver::shared_ptr createPCGSolver(const PCGSolverParameters ¶meters){ +// +// PCGSolver::shared_ptr solver; +// switch ( parameters.pcg_kernel_ ) { +// case PCGSolverParameters::PCG: +// switch ( parameters.blas_kernel_ ) { +// +//// case PCGSolverParameters::GTSAM: +//// solver.reset(new PCGSolver_FG(parameters)); +//// break; +// +// case PCGSolverParameters::SBM: +// case PCGSolverParameters::SBM_MT: +// solver.reset(new PCGSolver_SBM(parameters)); +// break; +// +// default: +// throw std::invalid_argument("createPCGSolver: undefined blas_kernel for pcgsolver"); +// break; +// } +// break; +// +// case PCGSolverParameters::LSPCG: +// switch ( parameters.blas_kernel_ ) { +// case PCGSolverParameters::GTSAM: +// solver.reset(new LSPCGSolver_FG(parameters)); +// break; +// +// case PCGSolverParameters::SBM: +// case PCGSolverParameters::SBM_MT: +// solver.reset(new LSPCGSolver_SBM(parameters)); +// break; +// +// default: +// throw std::invalid_argument("createPCGSolver: undefined blas_kernel for lspcg solver"); +// break; +// } +// break; +// +// default: +// throw std::invalid_argument("createPCGSolver: undefined pcg_kernel"); +// break; +// } +// return solver; +//} +} diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h new file mode 100644 index 000000000..034352c50 --- /dev/null +++ b/gtsam/linear/PCGSolver.h @@ -0,0 +1,236 @@ +/* + * PCGSolver.h + * + * Created on: Jan 31, 2012 + * Author: Yong-Dian Jian + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + +class GaussianFactorGraph; +class KeyInfo; +class Preconditioner; +class PreconditionerParameters; + +/*****************************************************************************/ +struct PCGSolverParameters: public ConjugateGradientParameters { +public: + typedef ConjugateGradientParameters Base; + typedef boost::shared_ptr shared_ptr; + + PCGSolverParameters() {} + + virtual void print(std::ostream &os) const; + + /* interface to preconditioner parameters */ + inline const PreconditionerParameters& preconditioner() const { + return *preconditioner_; + } + + boost::shared_ptr preconditioner_; +}; + +/*****************************************************************************/ +/* A virtual base class for the preconditioned conjugate gradient solver */ +class PCGSolver: public IterativeSolver { +public: + typedef IterativeSolver Base; + typedef boost::shared_ptr shared_ptr; + +protected: + + PCGSolverParameters parameters_; + boost::shared_ptr preconditioner_; + +// /* cached local variables */ +// KeyInfo keyInfo_; +// +// /* whether the preconditioner has be built */ +// bool built_ ; + +public: + /* Interface to initialize a solver without a problem */ + PCGSolver(const PCGSolverParameters &p); + virtual ~PCGSolver() {} + +// /* update interface to the nonlinear optimizer */ +// virtual void replaceFactors( +// const GaussianFactorGraph &factorGraph, +// const Values &linearization_point, +// const std::map lambda); + +// /* build the preconditioner */ +// void buildPreconditioner(); + +// /* interface for the NonlinearOptimizer, with initial estimate equals to a zero vector */ +// virtual VectorValues optimize() ; +// +// /* interface for the NonlinearOptimizer, with a custom initial estimate*/ +// virtual VectorValues optimize(const VectorValues &initial); + + using IterativeSolver::optimize; + + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial); + +//protected: + +// /* convert jacobian factor graph to solver-specific kernel */ +// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0) = 0 ; +// +// /* do the actual optimization */ +// virtual VectorValues optimize_(const VectorValues &initial) = 0; + +}; + +/*****************************************************************************/ +class GaussianFactorGraphSystem { +public: + + GaussianFactorGraphSystem(const GaussianFactorGraph &gfg, + const Preconditioner &preconditioner, const KeyInfo &info, + const std::map &lambda); + + const GaussianFactorGraph &gfg_; + const Preconditioner &preconditioner_; + const KeyInfo &keyInfo_; + const std::map &lambda_; + + void residual(const Vector &x, Vector &r) const; + void multiply(const Vector &x, Vector& y) const; + void leftPrecondition(const Vector &x, Vector &y) const; + void rightPrecondition(const Vector &x, Vector &y) const; + inline void scal(const double alpha, Vector &x) const { + x *= alpha; + } + inline double dot(const Vector &x, const Vector &y) const { + return x.dot(y); + } + inline void axpy(const double alpha, const Vector &x, Vector &y) const { + y += alpha * x; + } + + void getb(Vector &b) const; +}; + +/* utility functions */ +/**********************************************************************************/ +VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, + const std::map & dimensions); + +/**********************************************************************************/ +VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo); + +///*****************************************************************************/ +///* an specialization of the PCGSolver using gtsam's factor graph and linear algebra facility */ +//class PCGSolver_FG : public PCGSolver { +// +//public: +// typedef PCGSolver Base; +// typedef boost::shared_ptr shared_ptr ; +// +//protected: +// JacobianFactorGraph::shared_ptr hfg_; // A'*A + lambda*I +// +//public: +// PCGSolver_FG(const Parameters ¶meters) : Base(parameters) {} +// virtual ~PCGSolver_FG() {} +// +//protected: +// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0); +// virtual VectorValues optimize_(const VectorValues &initial); +// +// /* interface to the preconditionedConjugateGradient function template */ +// class System { +// public: +// typedef Vector State; +// typedef Vector Residual; +// +// protected: +// const JacobianFactorGraph::shared_ptr hfg_; +// const Preconditioner::shared_ptr preconditioner_; +// const KeyInfo &keyInfo_; +// const RowInfo &rowInfo_; +// +// public: +// System(const JacobianFactorGraph::shared_ptr hfg, +// const Preconditioner::shared_ptr preconditioner, +// const KeyInfo &keyInfo, const RowInfo &rowInfo); +// +// inline void residual(const State &x, State &r) const { gtsam::residual(*hfg_, x, r); } +// inline void multiply(const State &x, State& y) const { gtsam::multiply(*hfg_, x, y); } +// inline void leftPrecondition(const State &x, State &y) const { preconditioner_->transposeSolve(x, y); } +// inline void rightPrecondition(const State &x, State &y) const { preconditioner_->solve(x, y); } +// inline void scal(const double alpha, State &x) const { x.vector() *= alpha; } +// inline double dot(const State &x, const State &y) const { return x.vector().dot(y.vector()); } +// inline void axpy(const double alpha, const State &x, State &y) const { y.vector() += alpha*x.vector(); } +// } ; +//}; +// +///*****************************************************************************/ +///** an specialization of the PCGSolver using sbm and customized blas kernel **/ +//class PCGSolver_SBM : public PCGSolver { +// +//public: +// typedef PCGSolver Base; +// typedef boost::shared_ptr shared_ptr ; +// +//protected: +// ydjian::SparseLinearSystem::shared_ptr linear_; +// +//public: +// PCGSolver_SBM(const Parameters ¶meters) : Base(parameters) {} +// virtual ~PCGSolver_SBM() {} +// +//protected: +// /* virtual function of the PCGSolver */ +// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0); +// virtual VectorValues optimize_(const VectorValues &initial); +// +// /* interface to the preconditionedConjugateGradient function template */ +// class System { +// +// public: +// typedef Vector State; +// +// protected: +// const ydjian::SparseLinearSystem::shared_ptr linear_; +// const Preconditioner::shared_ptr preconditioner_; +// +// public: +// System(const ydjian::SparseLinearSystem::shared_ptr linear, const Preconditioner::shared_ptr preconditioner) +// : linear_(linear), preconditioner_(preconditioner) {} +// inline void residual(const State &x, State &r) const { linear_->residual(x.data(), r.data(), false); } +// inline void multiply(const State &x, State& y) const { linear_->matrix()->multiply(x.data(), y.data(), false); } +// inline void leftPrecondition(const State &x, State &y) const { preconditioner_->transposeSolve(x, y); } +// inline void rightPrecondition(const State &x, State &y) const { preconditioner_->solve(x, y); } +// inline void scal(const double alpha, State &x) const { x *= alpha; } +// inline double dot(const State &x, const State &y) const { return x.dot(y); } +// inline void axpy(const double alpha, const State &x, State &y) const { y += alpha*x; } +// } ; +//}; +// +///* a factory method to instantiate PCGSolvers and its derivatives */ +//PCGSolver::shared_ptr createPCGSolver(const PCGParameters ¶meters); +// +///* an utility function to create sparse linear system, for all cg solvers using the sbm kernel */ +//ydjian::SparseLinearSystem::shared_ptr +//buildSparseLinearSystem(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, const bool AtA, +// const double lambda, const bool colMajor, const PCGParameters::BLASKernel blas, +// const PCGParameters::RegisterBlockKernel rb); + +} + diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp new file mode 100644 index 000000000..c233c431b --- /dev/null +++ b/gtsam/linear/Preconditioner.cpp @@ -0,0 +1,212 @@ +/* + * Preconditioner.cpp + * + * Created on: Feb 2, 2012 + * Author: ydjian + */ + + +//#include +#include +//#include +#include +#include +//#include +//#include +//#include +#include +//#include +//#include +//#include +//#include +//#include +//#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + + +/*****************************************************************************/ +void PreconditionerParameters::print() const { + print(cout); +} + +/***************************************************************************************/ +void PreconditionerParameters::print(ostream &os) const { + os << "PreconditionerParameters" << endl + << "kernel: " << kernelTranslator(kernel_) << endl + << "verbosity: " << verbosityTranslator(verbosity_) << endl; +} + +/*****************************************************************************/ + ostream& operator<<(ostream &os, const PreconditionerParameters &p) { + p.print(os); + return os; +} + +/***************************************************************************************/ +PreconditionerParameters::Kernel PreconditionerParameters::kernelTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "GTSAM") return PreconditionerParameters::GTSAM; + else if (s == "CHOLMOD") return PreconditionerParameters::CHOLMOD; + /* default is cholmod */ + else return PreconditionerParameters::CHOLMOD; +} + +/***************************************************************************************/ +PreconditionerParameters::Verbosity PreconditionerParameters::verbosityTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "SILENT") return PreconditionerParameters::SILENT; + else if (s == "COMPLEXITY") return PreconditionerParameters::COMPLEXITY; + else if (s == "ERROR") return PreconditionerParameters::ERROR; + /* default is default */ + else return PreconditionerParameters::SILENT; +} + +/***************************************************************************************/ +std::string PreconditionerParameters::kernelTranslator(PreconditionerParameters::Kernel k) { + if ( k == GTSAM ) return "GTSAM"; + if ( k == CHOLMOD ) return "CHOLMOD"; + else return "UNKNOWN"; +} + +/***************************************************************************************/ +std::string PreconditionerParameters::verbosityTranslator(PreconditionerParameters::Verbosity verbosity) { + if (verbosity == SILENT) return "SILENT"; + else if (verbosity == COMPLEXITY) return "COMPLEXITY"; + else if (verbosity == ERROR) return "ERROR"; + else return "UNKNOWN"; +} + +///***************************************************************************************/ +//void Preconditioner::replaceFactors(const JacobianFactorGraph &jfg, const double lambda) +//{ +// const Parameters &p = *parameters_; +// +// if ( keyInfo_.size() == 0 ) { +// keyInfo_ = KeyInfo(jfg, *orderingNatural(jfg)); +// } +// +// if ( p.verbosity() >= Parameters::COMPLEXITY ) +// cout << "Preconditioner::replaceFactors with a jfg of " << jfg.size() << " factors."<< endl; +//} +// +/***************************************************************************************/ +boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters) { + + DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast(parameters); + if ( dummy ) { + return boost::make_shared(); + } + +// BlockJacobiPreconditioner::Parameters::shared_ptr jacobi = boost::dynamic_pointer_cast(parameters); +// if ( jacobi ) { +// BlockJacobiPreconditioner::shared_ptr p(new BlockJacobiPreconditioner(jacobi)); +// return p; +// } +// +// MIQRPreconditioner::Parameters::shared_ptr miqr = boost::dynamic_pointer_cast(parameters); +// if ( miqr ) { +// MIQRPreconditioner::shared_ptr p(new MIQRPreconditioner(miqr)); +// return p; +// } +// +// CombinatorialPreconditioner::Parameters::shared_ptr combinatorial +// = boost::dynamic_pointer_cast(parameters); +// if ( combinatorial ) { +// return createCombinatorialPreconditioner(combinatorial); +// } + + throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type"); +} +// +///***************************************************************************************/ +//JacobianFactorGraph::shared_ptr +//buildFactorSubgraph(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, +// const Subgraph &subgraph, const int hessian, const double lambda, const bool clone, const bool includeUnary){ +// +// const Subgraph::Edges &edges = subgraph.edges(); +// const size_t m = jfg.size(), n = keyInfo.size(); +// const bool augment = (lambda!=0.0) ? true : false ; +// +// /* produce an edge weight table */ +// std::vector weights(m, 0.0); +// BOOST_FOREACH ( const Subgraph::Edge &edge, edges ) { +// weights[edge.index()] = edge.weight(); +// } +// +// /* collect the number of dangling unary factors */ +// /* upper bound of the factors */ +// size_t extraUnary = 0; +// if ( includeUnary ) { +// for ( Index i = 0 ; i < m ; ++i ) { +// if ( weights[i] == 0.0 && jfg[i]->size() == 1 ) { +// weights[i] = 1.0; +// ++extraUnary; +// } +// } +// } +// +// const size_t e = edges.size() + extraUnary; +// const size_t sz = e + (augment ? n : 0) + (hessian ? 2*(m-e) : 0); +// +// JacobianFactorGraph::shared_ptr target(new JacobianFactorGraph()); +// target->reserve(sz); +// +// /* auxiliary variable */ +// size_t r = jfg[0]->rows(); +// SharedDiagonal sigma(noiseModel::Unit::Create(r)); +// +// for ( Index i = 0 ; i < m ; ++i ) { +// +// const double w = weights[i]; +// +// if ( w != 0.0 ) { /* subgraph edge */ +// if ( !clone && w == 1.0 ) { +// target->push_back(jfg[i]); +// } +// else { +// JacobianFactor::shared_ptr jf (new JacobianFactor(*jfg[i])); +// scaleJacobianFactor(*jf, w); +// target->push_back(jf); +// } +// } +// else { /* non-subgraph edge */ +// if ( hessian ) { +// const JacobianFactor &f = *jfg[i]; +// /* simple case: unary factor */ +// if ( f.size() == 1 ) { +// if (!clone) target->push_back(jfg[i]); +// else { +// JacobianFactor::shared_ptr jf(new JacobianFactor(*jfg[i])); +// target->push_back(jf); +// } +// } +// else { /* general factor */ +// const size_t rj = f.rows(); +// if ( rj != r ) { +// r = rj; sigma = noiseModel::Unit::Create(r); +// } +// for ( JacobianFactor::const_iterator j = f.begin() ; j != f.end() ; ++j ) { +// JacobianFactor::shared_ptr jf(new JacobianFactor(*j, f.getA(j), Vector::Zero(r,1), sigma)); +// target->push_back(jf); +// } /* for */ +// } /* factor arity */ +// } /* hessian */ +// } /* if w != 0.0 */ +// } /* for */ +// +// if ( !augment ) return target ; +// +// return appendPriorFactors(*target, keyInfo, lambda, false /* clone */); +//} + + +} + + diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h new file mode 100644 index 000000000..5ca5ec70b --- /dev/null +++ b/gtsam/linear/Preconditioner.h @@ -0,0 +1,173 @@ +/* + * Preconditioner.h + * + * Created on: Feb 1, 2012 + * Author: ydjian + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +class GaussianFactorGraph; +class KeyInfo; +class VectorValues; +//class Subgraph; + +/* parameters for the preconditioner */ +struct PreconditionerParameters { + + typedef boost::shared_ptr shared_ptr; + + enum Kernel { /* Preconditioner Kernel */ + GTSAM = 0, + CHOLMOD /* experimental */ + } kernel_ ; + + enum Verbosity { + SILENT = 0, + COMPLEXITY = 1, + ERROR = 2 + } verbosity_ ; + + PreconditionerParameters(): kernel_(GTSAM), verbosity_(SILENT) {} + PreconditionerParameters(const PreconditionerParameters &p) : kernel_(p.kernel_), verbosity_(p.verbosity_) {} + virtual ~PreconditionerParameters() {} + + /* general interface */ + inline Kernel kernel() const { return kernel_; } + inline Verbosity verbosity() const { return verbosity_; } + + void print() const ; + + virtual void print(std::ostream &os) const ; + + static Kernel kernelTranslator(const std::string &s); + static Verbosity verbosityTranslator(const std::string &s); + static std::string kernelTranslator(Kernel k); + static std::string verbosityTranslator(Verbosity v); + + /* for serialization */ + friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p); + + }; + + +/* PCG aims to solve the problem: A x = b by reparametrizing it as + * S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M + * The goal of this class is to provide a general interface to all preconditioners */ +class Preconditioner { +public: + typedef boost::shared_ptr shared_ptr; + typedef std::vector Dimensions; + + /* Generic Constructor and Destructor */ + Preconditioner() {} + virtual ~Preconditioner() {} + + /* Computation Interfaces */ + + /* implement x = S^{-1} y */ + virtual void solve(const Vector& y, Vector &x) const = 0; + virtual void solve(const VectorValues& y, VectorValues &x) const = 0; + + /* implement x = S^{-T} y */ + virtual void transposeSolve(const Vector& y, Vector& x) const = 0; + virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; + + /* implement x = S^{-1} S^{-T} y */ + virtual void fullSolve(const Vector& y, Vector &x) const = 0; + virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; + + /* build/factorize the preconditioner */ + virtual void build( + const GaussianFactorGraph &gfg, + const KeyInfo &info, + const std::map &lambda + ) = 0; + +// /* complexity index */ +// virtual size_t complexity() const = 0; +// +// /* is the preconditioner dependent to data */ +// virtual bool isStatic() const = 0; +// +// /* is the preconditioner kind of spanning subgraph preconditioner */ +// virtual bool isSubgraph() const = 0; +// +// /* return A\b */ +// virtual void xstar(Vector &result) const = 0 ; +// +//protected: +// Parameters::shared_ptr parameters_; +// KeyInfo keyInfo_; + +}; + +/*******************************************************************************************/ +struct DummyPreconditionerParameters : public PreconditionerParameters { + typedef PreconditionerParameters Base; + typedef boost::shared_ptr shared_ptr; + DummyPreconditionerParameters() : Base() {} + virtual ~DummyPreconditionerParameters() {} +}; + +/*******************************************************************************************/ +class DummyPreconditioner : public Preconditioner { +public: + typedef Preconditioner Base; + typedef boost::shared_ptr shared_ptr; + +public: + + DummyPreconditioner() : Base() {} + virtual ~DummyPreconditioner() {} + + /* Computation Interfaces for raw vector */ + virtual void solve(const Vector& y, Vector &x) const { x = y; } + virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; } + + virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; } + virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; } + + virtual void fullSolve(const Vector& y, Vector &x) const { x = y; } + virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; } + + virtual void build( + const GaussianFactorGraph &gfg, + const KeyInfo &info, + const std::map &lambda + ) {} + +// virtual void replaceFactors(const JacobianFactorGraph &jfg, const double lambda = 0.0) { +// Base::replaceFactors(jfg,lambda); +// } +// virtual void buildPreconditioner() {} +// virtual size_t complexity() const { return 0; } +// virtual bool isStatic() const { return true; } +// virtual bool isSubgraph() const { return false; } +// virtual void xstar(Vector &result) const {} +}; + +/* factory method to create preconditioners */ +boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters); + +///* build a factor subgraph, which is defined as a set of weighted edges (factors). +// * "hessian"={0,1} indicates whether the non-subgraph edges are split into multiple unary factors +// * "lambda" indicates whether scaled identity matrix is augmented +// * "clone" indicates whether factors are cloned into the subgraph if the edge has weight = 1.0 +// * "includeUnary" indicates whether the unary factors are forced to join the subgraph: +// * useful for static subgraph preconditioner, because unary factors were imposed dynamically by LM */ +//boost::shared_ptr +//buildFactorSubgraph(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, +// const Subgraph &subgraph, const int hessian, const double lambda, const bool clone, const bool includeUnary); + +} + + diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index f6557a2c2..a66182281 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -15,8 +15,8 @@ #include #include #include - #include +#include namespace gtsam { @@ -28,7 +28,7 @@ class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters public: typedef ConjugateGradientParameters Base; SubgraphSolverParameters() : Base() {} - virtual void print() const { Base::print(); } + virtual void print(std::ostream &os) const { Base::print(os); } }; /** @@ -77,8 +77,17 @@ public: SubgraphSolver(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); virtual ~SubgraphSolver() {} - virtual VectorValues optimize () ; - virtual VectorValues optimize (const VectorValues &initial) ; + + VectorValues optimize () ; + VectorValues optimize (const VectorValues &initial) ; + + /* interface to the nonlinear optimizer that the subclasses have to implement */ + virtual VectorValues optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial + ) { return VectorValues(); } protected: diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 678cac6b4..2a6eb5887 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -100,19 +101,18 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, if (params.isMultifrontal()) { delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); } else if (params.isSequential()) { - delta = gfg.eliminateSequential(*params.ordering, - params.getEliminationFunction())->optimize(); + delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); } else if (params.isIterative()) { if (!params.iterativeParams) - throw std::runtime_error( - "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); - if (boost::dynamic_pointer_cast( - params.iterativeParams)) { - SubgraphSolver solver(gfg, - dynamic_cast(*params.iterativeParams), - *params.ordering); - delta = solver.optimize(); - } else { + throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ..."); + + if (boost::shared_ptr pcg = boost::dynamic_pointer_cast(params.iterativeParams) ) { + delta = PCGSolver(*pcg).optimize(gfg); + } + else if (boost::shared_ptr spcg = boost::dynamic_pointer_cast(params.iterativeParams) ) { + delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize(); + } + else { throw std::runtime_error( "NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ..."); } diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp new file mode 100644 index 000000000..ea9b5ec6c --- /dev/null +++ b/tests/testPCGSolver.cpp @@ -0,0 +1,77 @@ +/* ---------------------------------------------------------------------------- + + * 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 testPCGSolver.cpp + * @brief Unit tests for PCGSolver class + * @author Yong-Dian Jian + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include // for operator += +using namespace boost::assign; + +#include +#include + +using namespace std; +using namespace gtsam; + +const double tol = 1e-3; + +using symbol_shorthand::X; +using symbol_shorthand::L; + +/* ************************************************************************* */ +TEST( NonlinearOptimizer, optimization_method ) +{ + LevenbergMarquardtParams paramsPCG; + paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; + PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->preconditioner_ = boost::make_shared(); + paramsPCG.iterativeParams = pcg; + + NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); + + Point2 x0(10,10); + Values c0; + c0.insert(X(1), x0); + + Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); + + DOUBLES_EQUAL(0,fg.error(actualPCG),tol); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} + From 67398f0f13eda8d4b4c89b17bb7c070df02dd82c Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Sun, 8 Jun 2014 16:15:00 -0400 Subject: [PATCH 80/90] add BlockJacobiPreconditioner class and unit test --- gtsam/linear/Preconditioner.cpp | 142 ++++++++++++++++++++++++++++---- gtsam/linear/Preconditioner.h | 81 ++++++++++-------- tests/testPCGSolver.cpp | 62 +++++++++++++- 3 files changed, 233 insertions(+), 52 deletions(-) diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index c233c431b..42ea56e68 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -9,6 +9,7 @@ //#include #include //#include +#include #include #include //#include @@ -83,26 +84,137 @@ std::string PreconditionerParameters::verbosityTranslator(PreconditionerParamete else return "UNKNOWN"; } -///***************************************************************************************/ -//void Preconditioner::replaceFactors(const JacobianFactorGraph &jfg, const double lambda) -//{ -// const Parameters &p = *parameters_; -// -// if ( keyInfo_.size() == 0 ) { -// keyInfo_ = KeyInfo(jfg, *orderingNatural(jfg)); -// } -// -// if ( p.verbosity() >= Parameters::COMPLEXITY ) -// cout << "Preconditioner::replaceFactors with a jfg of " << jfg.size() << " factors."<< endl; -//} -// +/***************************************************************************************/ +BlockJacobiPreconditioner::BlockJacobiPreconditioner() + : Base(), buffer_(0), bufferSize_(0), nnz_(0) {} + +/***************************************************************************************/ +BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { clean(); } + +/***************************************************************************************/ +void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const { + + const size_t n = dims_.size(); + double *ptr = buffer_, *dst = x.data(); + + std::copy(y.data(), y.data() + y.rows(), x.data()); + + for ( size_t i = 0 ; i < n ; ++i ) { + const size_t d = dims_[i]; + const size_t sz = d*d; + + const Eigen::Map R(ptr, d, d); + Eigen::Map b(dst, d, 1); + R.triangularView().solveInPlace(b); + + dst += d; + ptr += sz; + } +} + +/***************************************************************************************/ +void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const { + const size_t n = dims_.size(); + double *ptr = buffer_, *dst = x.data(); + + std::copy(y.data(), y.data() + y.rows(), x.data()); + + for ( size_t i = 0 ; i < n ; ++i ) { + const size_t d = dims_[i]; + const size_t sz = d*d; + + const Eigen::Map R(ptr, d, d); + Eigen::Map b(dst, d, 1); + R.transpose().triangularView().solveInPlace(b); + + dst += d; + ptr += sz; + } +} + +/***************************************************************************************/ +void BlockJacobiPreconditioner::build( + const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) +{ + const size_t n = keyInfo.size(); + dims_ = keyInfo.colSpec(); + + /* prepare the buffer of block diagonals */ + std::vector blocks; blocks.reserve(n); + + /* allocate memory for the factorization of block diagonals */ + size_t nnz = 0; + for ( size_t i = 0 ; i < n ; ++i ) { + const size_t dim = dims_[i]; + blocks.push_back(Matrix::Zero(dim, dim)); + // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ; + nnz += dim*dim; + } + + /* compute the block diagonal by scanning over the factors */ + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { + const KeyInfoEntry &entry = keyInfo.find(*it)->second; + const Matrix &Ai = jf->getA(it); + blocks[entry.index()] += (Ai.transpose() * Ai); + } + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) { + const KeyInfoEntry &entry = keyInfo.find(*it)->second; + const Matrix &Hii = hf->info(it, it).selfadjointView(); + blocks[entry.index()] += Hii; + } + } + else { + throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); + } + } + + /* if necessary, allocating the memory for cacheing the factorization results */ + if ( nnz > bufferSize_ ) { + clean(); + buffer_ = new double [nnz]; + bufferSize_ = nnz; + } + nnz_ = nnz; + + /* factorizing the blocks respectively */ + double *ptr = buffer_; + for ( size_t i = 0 ; i < n ; ++i ) { + /* use eigen to decompose Di */ + const Matrix R = blocks[i].llt().matrixL().transpose(); + + /* store the data in the buffer */ + size_t sz = dims_[i]*dims_[i] ; + std::copy(R.data(), R.data() + sz, ptr); + + /* advance the pointer */ + ptr += sz; + } +} + +/*****************************************************************************/ +void BlockJacobiPreconditioner::clean() { + if ( buffer_ ) { + delete [] buffer_; + buffer_ = 0; + bufferSize_ = 0; + nnz_ = 0; + } +} + /***************************************************************************************/ boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters) { - DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast(parameters); - if ( dummy ) { + if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast(parameters) ) { return boost::make_shared(); } + else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast(parameters) ) { + return boost::make_shared(); + } + // BlockJacobiPreconditioner::Parameters::shared_ptr jacobi = boost::dynamic_pointer_cast(parameters); // if ( jacobi ) { diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 5ca5ec70b..17c12446f 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -18,7 +18,6 @@ namespace gtsam { class GaussianFactorGraph; class KeyInfo; class VectorValues; -//class Subgraph; /* parameters for the preconditioner */ struct PreconditionerParameters { @@ -75,15 +74,15 @@ public: /* implement x = S^{-1} y */ virtual void solve(const Vector& y, Vector &x) const = 0; - virtual void solve(const VectorValues& y, VectorValues &x) const = 0; +// virtual void solve(const VectorValues& y, VectorValues &x) const = 0; /* implement x = S^{-T} y */ virtual void transposeSolve(const Vector& y, Vector& x) const = 0; - virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; +// virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; - /* implement x = S^{-1} S^{-T} y */ - virtual void fullSolve(const Vector& y, Vector &x) const = 0; - virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; +// /* implement x = S^{-1} S^{-T} y */ +// virtual void fullSolve(const Vector& y, Vector &x) const = 0; +// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; /* build/factorize the preconditioner */ virtual void build( @@ -91,23 +90,6 @@ public: const KeyInfo &info, const std::map &lambda ) = 0; - -// /* complexity index */ -// virtual size_t complexity() const = 0; -// -// /* is the preconditioner dependent to data */ -// virtual bool isStatic() const = 0; -// -// /* is the preconditioner kind of spanning subgraph preconditioner */ -// virtual bool isSubgraph() const = 0; -// -// /* return A\b */ -// virtual void xstar(Vector &result) const = 0 ; -// -//protected: -// Parameters::shared_ptr parameters_; -// KeyInfo keyInfo_; - }; /*******************************************************************************************/ @@ -131,30 +113,57 @@ public: /* Computation Interfaces for raw vector */ virtual void solve(const Vector& y, Vector &x) const { x = y; } - virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; } +// virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; } virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; } - virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; } +// virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; } - virtual void fullSolve(const Vector& y, Vector &x) const { x = y; } - virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; } +// virtual void fullSolve(const Vector& y, Vector &x) const { x = y; } +// virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; } virtual void build( const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map &lambda ) {} - -// virtual void replaceFactors(const JacobianFactorGraph &jfg, const double lambda = 0.0) { -// Base::replaceFactors(jfg,lambda); -// } -// virtual void buildPreconditioner() {} -// virtual size_t complexity() const { return 0; } -// virtual bool isStatic() const { return true; } -// virtual bool isSubgraph() const { return false; } -// virtual void xstar(Vector &result) const {} }; +/*******************************************************************************************/ +struct BlockJacobiPreconditionerParameters : public PreconditionerParameters { + typedef PreconditionerParameters Base; + BlockJacobiPreconditionerParameters() : Base() {} + virtual ~BlockJacobiPreconditionerParameters() {} +}; + +/*******************************************************************************************/ +class BlockJacobiPreconditioner : public Preconditioner { +public: + typedef Preconditioner Base; + BlockJacobiPreconditioner() ; + virtual ~BlockJacobiPreconditioner() ; + + /* Computation Interfaces for raw vector */ + virtual void solve(const Vector& y, Vector &x) const; + virtual void transposeSolve(const Vector& y, Vector& x) const ; +// virtual void fullSolve(const Vector& y, Vector &x) const ; + + virtual void build( + const GaussianFactorGraph &gfg, + const KeyInfo &info, + const std::map &lambda + ) ; + +protected: + + void clean() ; + + std::vector dims_; + double *buffer_; + size_t bufferSize_; + size_t nnz_; +}; + +/*********************************************************************************************/ /* factory method to create preconditioners */ boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters); diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index ea9b5ec6c..ef74ad1ef 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -50,7 +50,47 @@ using symbol_shorthand::X; using symbol_shorthand::L; /* ************************************************************************* */ -TEST( NonlinearOptimizer, optimization_method ) +TEST( PCGSolver, llt ) { + Matrix R = (Matrix(3,3) << + 1., -1., -1., + 0., 2., -1., + 0., 0., 1.); + Matrix AtA = R.transpose() * R; + + Vector Rvector = (Vector(9) << 1., -1., -1., + 0., 2., -1., + 0., 0., 1.); +// Vector Rvector = (Vector(6) << 1., -1., -1., +// 2., -1., +// 1.); + + Vector b = (Vector(3) << 1., 2., 3.); + + Vector x = (Vector(3) << 6.5, 2.5, 3.) ; + + /* test cholesky */ + Matrix Rhat = AtA.llt().matrixL().transpose(); + EXPECT(assert_equal(R, Rhat, 1e-5)); + + /* test backward substitution */ + Vector xhat = Rhat.triangularView().solve(b); + EXPECT(assert_equal(x, xhat, 1e-5)); + + /* test in-place back substitution */ + xhat = b; + Rhat.triangularView().solveInPlace(xhat); + EXPECT(assert_equal(x, xhat, 1e-5)); + + /* test triangular matrix map */ + Eigen::Map Radapter(Rvector.data(), 3, 3); + xhat = Radapter.transpose().triangularView().solve(b); + EXPECT(assert_equal(x, xhat, 1e-5)); + +} + + +/* ************************************************************************* */ +TEST( PCGSolver, dummy ) { LevenbergMarquardtParams paramsPCG; paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; @@ -69,6 +109,26 @@ TEST( NonlinearOptimizer, optimization_method ) DOUBLES_EQUAL(0,fg.error(actualPCG),tol); } +/* ************************************************************************* */ +TEST( PCGSolver, blockjacobi ) +{ + LevenbergMarquardtParams paramsPCG; + paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; + PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->preconditioner_ = boost::make_shared(); + paramsPCG.iterativeParams = pcg; + + NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); + + Point2 x0(10,10); + Values c0; + c0.insert(X(1), x0); + + Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); + + DOUBLES_EQUAL(0,fg.error(actualPCG),tol); +} + /* ************************************************************************* */ int main() { TestResult tr; From 17426d0076dfbb35742c413b1947f8ba90fc2fda Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Sun, 15 Jun 2014 23:14:06 -0400 Subject: [PATCH 81/90] subgraph preconditioner revised --- gtsam/linear/ConjugateGradientSolver.h | 2 +- gtsam/linear/IterativeSolver.h | 4 +- gtsam/linear/PCGSolver.cpp | 236 ------ gtsam/linear/PCGSolver.h | 133 +--- gtsam/linear/Preconditioner.cpp | 120 +-- gtsam/linear/Preconditioner.h | 26 +- gtsam/linear/SubgraphPreconditioner.cpp | 722 +++++++++++++++--- gtsam/linear/SubgraphPreconditioner.h | 207 ++++- gtsam/linear/SubgraphSolver.cpp | 8 + gtsam/linear/SubgraphSolver.h | 6 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 1 + gtsam/nonlinear/NonlinearOptimizer.cpp | 1 + tests/testPCGSolver.cpp | 22 +- 13 files changed, 873 insertions(+), 615 deletions(-) diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 47ec4b405..2382a0eb1 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -19,7 +19,7 @@ namespace gtsam { /** * parameters for the conjugate gradient method */ -class ConjugateGradientParameters : public IterativeOptimizationParameters { +class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationParameters { public: typedef IterativeOptimizationParameters Base; diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 66006875b..e02d92e3a 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -115,6 +115,7 @@ namespace gtsam { class KeyInfo : public std::map { public: typedef std::map Base; + KeyInfo() {} KeyInfo(const GaussianFactorGraph &fg); KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); @@ -132,9 +133,6 @@ namespace gtsam { Ordering ordering_; size_t numCols_; - private: - KeyInfo() {} - }; diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 6bfa389fb..27eb57b44 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -198,240 +198,4 @@ VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { return result; } -///*****************************************************************************/ -//std::string PCGSolverParameters::pcgTranslator(const PCGKernel value) { -// std::string s; -// switch (value) { -// case PCGSolverParameters::PCG: s = "PCG"; break; -// case PCGSolverParameters::LSPCG: s = "LSPCG"; break; -// case PCGSolverParameters::SPCG: s = "SPCG"; break; -// default: s = "UNDEFINED"; break; -// } -// return s; -//} -// -///*****************************************************************************/ -//PCGSolverParameters::PCGKernel PCGSolverParameters::pcgTranslator(const std::string &src) { -// std::string s = src; boost::algorithm::to_upper(s); -// if (s == "PCG") return PCGSolverParameters::PCG; -// if (s == "LSPCG") return PCGSolverParameters::LSPCG; -// if (s == "SPCG") return PCGSolverParameters::SPCG; -// -// /* default is LSPCG */ -// return PCGSolverParameters::LSPCG; -//} -// -///*****************************************************************************/ -//std::string PCGSolverParameters::blasTranslator(const BLASKernel value) { -// std::string s; -// switch (value) { -// case PCGSolverParameters::GTSAM: s = "GTSAM" ; break; -// case PCGSolverParameters::SBM: s = "SBM" ; break; -// case PCGSolverParameters::SBM_MT: s = "SBM-MT" ; break; -// default: s = "UNDEFINED" ; break; -// } -// return s; -//} -// -///*****************************************************************************/ -//PCGSolverParameters::BLASKernel PCGSolverParameters::blasTranslator(const std::string &src) { -// std::string s = src; boost::algorithm::to_upper(s); -// if (s == "GTSAM") return PCGSolverParameters::GTSAM; -// if (s == "SBM") return PCGSolverParameters::SBM; -// if (s == "SBM-MT") return PCGSolverParameters::SBM_MT; -// -// /* default is SBM */ -// return PCGSolverParameters::SBM; -//} -// -///*****************************************************************************/ -//std::string PCGSolverParameters::rbTranslator(const RegisterBlockKernel k) { -// std::string s; -// switch (k) { -// case PCGSolverParameters::RB_NONE: s = "RB_NONE" ; break; -// case PCGSolverParameters::SSE_RB22: s = "SSE_RB22" ; break; -// case PCGSolverParameters::SSE_RB44: s = "SSE_RB44" ; break; -// case PCGSolverParameters::AVX_RB44: s = "AVX_RB44" ; break; -// default: s = "UNDEFINED" ; break; -// } -// return s; -//} -// -///*****************************************************************************/ -//PCGSolverParameters::RegisterBlockKernel PCGSolverParameters::rbTranslator(const std::string &src) { -// std::string s = src; boost::algorithm::to_upper(s); -// if (s == "") return PCGSolverParameters::RB_NONE; -// if (s == "SSE_RB22") return PCGSolverParameters::SSE_RB22; -// if (s == "SSE_RB44") return PCGSolverParameters::SSE_RB44; -// if (s == "AVX_RB44") return PCGSolverParameters::AVX_RB44; -// -// /* default is SBM */ -// return PCGSolverParameters::RB_NONE; -//} -// -///*****************************************************************************/ -//PCGSolver::PCGSolver(const PCGSolverParameters &p) : Base(), parameters_(p), built_(false) { -// preconditioner_ = createPreconditioner(p.preconditioner_); -//} -// -///*****************************************************************************/ -//void PCGSolver::replaceFactors( -// const Values &linearization_point, -// const GaussianFactorGraph &gfg, -// const double lambda) -//{ -// const JacobianFactorGraph jfg(gfg); -// -// /* prepare the column structure */ -// if ( keyInfo_.size() == 0 ) { -// keyInfo_ = KeyInfo(jfg, *orderingNatural(jfg)); -// } -// -// /* implemented by subclass */ -// safe_tic_(); -// convertKernel(jfg, lambda); -// safe_toc_("convert-kernel"); -// -// /* update the preconditioner as well */ -// preconditioner_->replaceFactors(jfg, lambda); -// built_ = false; -//} -// -///*****************************************************************************/ -//void PCGSolver::buildPreconditioner() { -// if ( built_ == false ) { -// safe_tic_(); -// preconditioner_->buildPreconditioner(); -// built_ = true; -// safe_toc_("factorize"); -// } -//} -// -///*****************************************************************************/ -//VectorValues PCGSolver::optimize() { -// -// buildPreconditioner(); -// -// VectorValues zero; -// BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo_ ) { -// zero.insert(item.first, Vector::Zero(item.second.dim())); -// } -// -// return optimize(zero); -//} -// -///*****************************************************************************/ -//VectorValues PCGSolver::optimize(const VectorValues &initial) { -// safe_tic_(); -// VectorValues result = optimize_(initial); -// safe_toc_("pcg"); -// return result ; -//} -// -///*****************************************************************************/ -////void PCGSolver_FG::convertKernel(const JacobianFactorGraph &jfg, const double lambda) { -//// hfg_ = buildOuterProductJacobianFactorGraph(jfg, VariableIndex(jfg), *x0_, lambda); -////} -//// -////VectorValues PCGSolver_FG::optimize_(const VectorValues &initial) { -//// System system(hfg_, preconditioner_); -//// return preconditionedConjugateGradient(system, initial, parameters_); -////} -// -///*****************************************************************************/ -//void PCGSolver_SBM::convertKernel(const JacobianFactorGraph &jfg, const double lambda) { -// linear_ = buildSparseLinearSystem(jfg, keyInfo_, true /* AtA */, lambda, false /* column major */, -// parameters_.blas_kernel_, parameters_.rb_kernel_); -//} -// -///*****************************************************************************/ -//VectorValues PCGSolver_SBM::optimize_(const VectorValues &initial) { -// System system(linear_, preconditioner_); -// Vector solution = preconditionedConjugateGradient( -// system, initial.vector(keyInfo_.ordering()), parameters_); -// return buildVectorValues(solution, keyInfo_); -//} -// -///*****************************************************************************/ -//ydjian::SparseLinearSystem::shared_ptr -//buildSparseLinearSystem(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, -// const bool AtA, const double lambda, const bool colMajor, -// const PCGSolverParameters::BLASKernel blas, const PCGSolverParameters::RegisterBlockKernel rbSrc) { -// -// std::map rbMap; -// rbMap[PCGSolverParameters::RB_NONE] = ydjian::SBM::NAIVE; -// rbMap[PCGSolverParameters::SSE_RB22] = ydjian::SBM::SSE_RB22; -// rbMap[PCGSolverParameters::SSE_RB44] = ydjian::SBM::SSE_RB44; -// rbMap[PCGSolverParameters::AVX_RB44] = ydjian::SBM::AVX_RB44; -// -// ydjian::SBM::Type rb; -// SC_ASSERT( rbMap.find(rbSrc) != rbMap.end(), "buildSparseLinearSystem: rbSrc is not supported.."); -// rb = rbMap[rbSrc]; -// -// ydjian::SparseLinearSystem::shared_ptr linear; -// -// switch (blas) { -// -// case PCGSolverParameters::SBM: -// linear = gsp2::buildSparseLinearSystem(jfg, keyInfo, AtA, lambda, colMajor, false /* multithread */, rb); -// break; -// -// case PCGSolverParameters::SBM_MT: -// linear = gsp2::buildSparseLinearSystem(jfg, keyInfo, AtA, lambda, colMajor, true /* multithread */, rb); -// break; -// -// default: -// throw std::invalid_argument("createSparseLinearSystem: unsupported blas kernel"); -// break; -// } -// -// return linear; -//} -// -///*****************************************************************************/ -//PCGSolver::shared_ptr createPCGSolver(const PCGSolverParameters ¶meters){ -// -// PCGSolver::shared_ptr solver; -// switch ( parameters.pcg_kernel_ ) { -// case PCGSolverParameters::PCG: -// switch ( parameters.blas_kernel_ ) { -// -//// case PCGSolverParameters::GTSAM: -//// solver.reset(new PCGSolver_FG(parameters)); -//// break; -// -// case PCGSolverParameters::SBM: -// case PCGSolverParameters::SBM_MT: -// solver.reset(new PCGSolver_SBM(parameters)); -// break; -// -// default: -// throw std::invalid_argument("createPCGSolver: undefined blas_kernel for pcgsolver"); -// break; -// } -// break; -// -// case PCGSolverParameters::LSPCG: -// switch ( parameters.blas_kernel_ ) { -// case PCGSolverParameters::GTSAM: -// solver.reset(new LSPCGSolver_FG(parameters)); -// break; -// -// case PCGSolverParameters::SBM: -// case PCGSolverParameters::SBM_MT: -// solver.reset(new LSPCGSolver_SBM(parameters)); -// break; -// -// default: -// throw std::invalid_argument("createPCGSolver: undefined blas_kernel for lspcg solver"); -// break; -// } -// break; -// -// default: -// throw std::invalid_argument("createPCGSolver: undefined pcg_kernel"); -// break; -// } -// return solver; -//} } diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index 034352c50..8d48da149 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -25,7 +25,7 @@ class Preconditioner; class PreconditionerParameters; /*****************************************************************************/ -struct PCGSolverParameters: public ConjugateGradientParameters { +struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; typedef boost::shared_ptr shared_ptr; @@ -44,7 +44,7 @@ public: /*****************************************************************************/ /* A virtual base class for the preconditioned conjugate gradient solver */ -class PCGSolver: public IterativeSolver { +class GTSAM_EXPORT PCGSolver: public IterativeSolver { public: typedef IterativeSolver Base; typedef boost::shared_ptr shared_ptr; @@ -54,50 +54,21 @@ protected: PCGSolverParameters parameters_; boost::shared_ptr preconditioner_; -// /* cached local variables */ -// KeyInfo keyInfo_; -// -// /* whether the preconditioner has be built */ -// bool built_ ; - public: /* Interface to initialize a solver without a problem */ PCGSolver(const PCGSolverParameters &p); virtual ~PCGSolver() {} -// /* update interface to the nonlinear optimizer */ -// virtual void replaceFactors( -// const GaussianFactorGraph &factorGraph, -// const Values &linearization_point, -// const std::map lambda); - -// /* build the preconditioner */ -// void buildPreconditioner(); - -// /* interface for the NonlinearOptimizer, with initial estimate equals to a zero vector */ -// virtual VectorValues optimize() ; -// -// /* interface for the NonlinearOptimizer, with a custom initial estimate*/ -// virtual VectorValues optimize(const VectorValues &initial); - using IterativeSolver::optimize; virtual VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, const VectorValues &initial); -//protected: - -// /* convert jacobian factor graph to solver-specific kernel */ -// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0) = 0 ; -// -// /* do the actual optimization */ -// virtual VectorValues optimize_(const VectorValues &initial) = 0; - }; /*****************************************************************************/ -class GaussianFactorGraphSystem { +class GTSAM_EXPORT GaussianFactorGraphSystem { public: GaussianFactorGraphSystem(const GaussianFactorGraph &gfg, @@ -134,103 +105,5 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, /**********************************************************************************/ VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo); -///*****************************************************************************/ -///* an specialization of the PCGSolver using gtsam's factor graph and linear algebra facility */ -//class PCGSolver_FG : public PCGSolver { -// -//public: -// typedef PCGSolver Base; -// typedef boost::shared_ptr shared_ptr ; -// -//protected: -// JacobianFactorGraph::shared_ptr hfg_; // A'*A + lambda*I -// -//public: -// PCGSolver_FG(const Parameters ¶meters) : Base(parameters) {} -// virtual ~PCGSolver_FG() {} -// -//protected: -// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0); -// virtual VectorValues optimize_(const VectorValues &initial); -// -// /* interface to the preconditionedConjugateGradient function template */ -// class System { -// public: -// typedef Vector State; -// typedef Vector Residual; -// -// protected: -// const JacobianFactorGraph::shared_ptr hfg_; -// const Preconditioner::shared_ptr preconditioner_; -// const KeyInfo &keyInfo_; -// const RowInfo &rowInfo_; -// -// public: -// System(const JacobianFactorGraph::shared_ptr hfg, -// const Preconditioner::shared_ptr preconditioner, -// const KeyInfo &keyInfo, const RowInfo &rowInfo); -// -// inline void residual(const State &x, State &r) const { gtsam::residual(*hfg_, x, r); } -// inline void multiply(const State &x, State& y) const { gtsam::multiply(*hfg_, x, y); } -// inline void leftPrecondition(const State &x, State &y) const { preconditioner_->transposeSolve(x, y); } -// inline void rightPrecondition(const State &x, State &y) const { preconditioner_->solve(x, y); } -// inline void scal(const double alpha, State &x) const { x.vector() *= alpha; } -// inline double dot(const State &x, const State &y) const { return x.vector().dot(y.vector()); } -// inline void axpy(const double alpha, const State &x, State &y) const { y.vector() += alpha*x.vector(); } -// } ; -//}; -// -///*****************************************************************************/ -///** an specialization of the PCGSolver using sbm and customized blas kernel **/ -//class PCGSolver_SBM : public PCGSolver { -// -//public: -// typedef PCGSolver Base; -// typedef boost::shared_ptr shared_ptr ; -// -//protected: -// ydjian::SparseLinearSystem::shared_ptr linear_; -// -//public: -// PCGSolver_SBM(const Parameters ¶meters) : Base(parameters) {} -// virtual ~PCGSolver_SBM() {} -// -//protected: -// /* virtual function of the PCGSolver */ -// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0); -// virtual VectorValues optimize_(const VectorValues &initial); -// -// /* interface to the preconditionedConjugateGradient function template */ -// class System { -// -// public: -// typedef Vector State; -// -// protected: -// const ydjian::SparseLinearSystem::shared_ptr linear_; -// const Preconditioner::shared_ptr preconditioner_; -// -// public: -// System(const ydjian::SparseLinearSystem::shared_ptr linear, const Preconditioner::shared_ptr preconditioner) -// : linear_(linear), preconditioner_(preconditioner) {} -// inline void residual(const State &x, State &r) const { linear_->residual(x.data(), r.data(), false); } -// inline void multiply(const State &x, State& y) const { linear_->matrix()->multiply(x.data(), y.data(), false); } -// inline void leftPrecondition(const State &x, State &y) const { preconditioner_->transposeSolve(x, y); } -// inline void rightPrecondition(const State &x, State &y) const { preconditioner_->solve(x, y); } -// inline void scal(const double alpha, State &x) const { x *= alpha; } -// inline double dot(const State &x, const State &y) const { return x.dot(y); } -// inline void axpy(const double alpha, const State &x, State &y) const { y += alpha*x; } -// } ; -//}; -// -///* a factory method to instantiate PCGSolvers and its derivatives */ -//PCGSolver::shared_ptr createPCGSolver(const PCGParameters ¶meters); -// -///* an utility function to create sparse linear system, for all cg solvers using the sbm kernel */ -//ydjian::SparseLinearSystem::shared_ptr -//buildSparseLinearSystem(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, const bool AtA, -// const double lambda, const bool colMajor, const PCGParameters::BLASKernel blas, -// const PCGParameters::RegisterBlockKernel rb); - } diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 42ea56e68..c180f1160 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -1,27 +1,16 @@ /* * Preconditioner.cpp * - * Created on: Feb 2, 2012 + * Created on: Jun 2, 2014 * Author: ydjian */ - -//#include #include -//#include #include #include #include -//#include -//#include -//#include +#include #include -//#include -//#include -//#include -//#include -//#include -//#include #include #include #include @@ -31,7 +20,6 @@ using namespace std; namespace gtsam { - /*****************************************************************************/ void PreconditionerParameters::print() const { print(cout); @@ -214,110 +202,12 @@ boost::shared_ptr createPreconditioner(const boost::shared_ptr

(parameters) ) { return boost::make_shared(); } - - -// BlockJacobiPreconditioner::Parameters::shared_ptr jacobi = boost::dynamic_pointer_cast(parameters); -// if ( jacobi ) { -// BlockJacobiPreconditioner::shared_ptr p(new BlockJacobiPreconditioner(jacobi)); -// return p; -// } -// -// MIQRPreconditioner::Parameters::shared_ptr miqr = boost::dynamic_pointer_cast(parameters); -// if ( miqr ) { -// MIQRPreconditioner::shared_ptr p(new MIQRPreconditioner(miqr)); -// return p; -// } -// -// CombinatorialPreconditioner::Parameters::shared_ptr combinatorial -// = boost::dynamic_pointer_cast(parameters); -// if ( combinatorial ) { -// return createCombinatorialPreconditioner(combinatorial); -// } + else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast(parameters) ) { + return boost::make_shared(*subgraph); + } throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type"); } -// -///***************************************************************************************/ -//JacobianFactorGraph::shared_ptr -//buildFactorSubgraph(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, -// const Subgraph &subgraph, const int hessian, const double lambda, const bool clone, const bool includeUnary){ -// -// const Subgraph::Edges &edges = subgraph.edges(); -// const size_t m = jfg.size(), n = keyInfo.size(); -// const bool augment = (lambda!=0.0) ? true : false ; -// -// /* produce an edge weight table */ -// std::vector weights(m, 0.0); -// BOOST_FOREACH ( const Subgraph::Edge &edge, edges ) { -// weights[edge.index()] = edge.weight(); -// } -// -// /* collect the number of dangling unary factors */ -// /* upper bound of the factors */ -// size_t extraUnary = 0; -// if ( includeUnary ) { -// for ( Index i = 0 ; i < m ; ++i ) { -// if ( weights[i] == 0.0 && jfg[i]->size() == 1 ) { -// weights[i] = 1.0; -// ++extraUnary; -// } -// } -// } -// -// const size_t e = edges.size() + extraUnary; -// const size_t sz = e + (augment ? n : 0) + (hessian ? 2*(m-e) : 0); -// -// JacobianFactorGraph::shared_ptr target(new JacobianFactorGraph()); -// target->reserve(sz); -// -// /* auxiliary variable */ -// size_t r = jfg[0]->rows(); -// SharedDiagonal sigma(noiseModel::Unit::Create(r)); -// -// for ( Index i = 0 ; i < m ; ++i ) { -// -// const double w = weights[i]; -// -// if ( w != 0.0 ) { /* subgraph edge */ -// if ( !clone && w == 1.0 ) { -// target->push_back(jfg[i]); -// } -// else { -// JacobianFactor::shared_ptr jf (new JacobianFactor(*jfg[i])); -// scaleJacobianFactor(*jf, w); -// target->push_back(jf); -// } -// } -// else { /* non-subgraph edge */ -// if ( hessian ) { -// const JacobianFactor &f = *jfg[i]; -// /* simple case: unary factor */ -// if ( f.size() == 1 ) { -// if (!clone) target->push_back(jfg[i]); -// else { -// JacobianFactor::shared_ptr jf(new JacobianFactor(*jfg[i])); -// target->push_back(jf); -// } -// } -// else { /* general factor */ -// const size_t rj = f.rows(); -// if ( rj != r ) { -// r = rj; sigma = noiseModel::Unit::Create(r); -// } -// for ( JacobianFactor::const_iterator j = f.begin() ; j != f.end() ; ++j ) { -// JacobianFactor::shared_ptr jf(new JacobianFactor(*j, f.getA(j), Vector::Zero(r,1), sigma)); -// target->push_back(jf); -// } /* for */ -// } /* factor arity */ -// } /* hessian */ -// } /* if w != 0.0 */ -// } /* for */ -// -// if ( !augment ) return target ; -// -// return appendPriorFactors(*target, keyInfo, lambda, false /* clone */); -//} - } diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 17c12446f..10ceb78a9 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -1,7 +1,7 @@ /* * Preconditioner.h * - * Created on: Feb 1, 2012 + * Created on: Jun 2, 2014 * Author: ydjian */ @@ -20,7 +20,7 @@ class KeyInfo; class VectorValues; /* parameters for the preconditioner */ -struct PreconditionerParameters { +struct GTSAM_EXPORT PreconditionerParameters { typedef boost::shared_ptr shared_ptr; @@ -54,14 +54,12 @@ struct PreconditionerParameters { /* for serialization */ friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p); - }; - /* PCG aims to solve the problem: A x = b by reparametrizing it as * S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M * The goal of this class is to provide a general interface to all preconditioners */ -class Preconditioner { +class GTSAM_EXPORT Preconditioner { public: typedef boost::shared_ptr shared_ptr; typedef std::vector Dimensions; @@ -93,7 +91,7 @@ public: }; /*******************************************************************************************/ -struct DummyPreconditionerParameters : public PreconditionerParameters { +struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParameters { typedef PreconditionerParameters Base; typedef boost::shared_ptr shared_ptr; DummyPreconditionerParameters() : Base() {} @@ -101,7 +99,7 @@ struct DummyPreconditionerParameters : public PreconditionerParameters { }; /*******************************************************************************************/ -class DummyPreconditioner : public Preconditioner { +class GTSAM_EXPORT DummyPreconditioner : public Preconditioner { public: typedef Preconditioner Base; typedef boost::shared_ptr shared_ptr; @@ -129,14 +127,14 @@ public: }; /*******************************************************************************************/ -struct BlockJacobiPreconditionerParameters : public PreconditionerParameters { +struct GTSAM_EXPORT BlockJacobiPreconditionerParameters : public PreconditionerParameters { typedef PreconditionerParameters Base; BlockJacobiPreconditionerParameters() : Base() {} virtual ~BlockJacobiPreconditionerParameters() {} }; /*******************************************************************************************/ -class BlockJacobiPreconditioner : public Preconditioner { +class GTSAM_EXPORT BlockJacobiPreconditioner : public Preconditioner { public: typedef Preconditioner Base; BlockJacobiPreconditioner() ; @@ -167,16 +165,6 @@ protected: /* factory method to create preconditioners */ boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters); -///* build a factor subgraph, which is defined as a set of weighted edges (factors). -// * "hessian"={0,1} indicates whether the non-subgraph edges are split into multiple unary factors -// * "lambda" indicates whether scaled identity matrix is augmented -// * "clone" indicates whether factors are cloned into the subgraph if the edge has weight = 1.0 -// * "includeUnary" indicates whether the unary factors are forced to join the subgraph: -// * useful for static subgraph preconditioner, because unary factors were imposed dynamically by LM */ -//boost::shared_ptr -//buildFactorSubgraph(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, -// const Subgraph &subgraph, const int hessian, const double lambda, const bool clone, const bool includeUnary); - } diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 44cb8c146..20b434b9f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -15,126 +15,646 @@ * @author: Frank Dellaert */ +#include #include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace std; namespace gtsam { - /* ************************************************************************* */ - static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { - JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); - if( !jf ) { - jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) +/* ************************************************************************* */ +static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); + if( !jf ) { + jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + result->push_back(jf); + } + return result; +} + +/*****************************************************************************/ +static std::vector iidSampler(const vector &weight, const size_t n) { + + /* compute the sum of the weights */ + const double sum = std::accumulate(weight.begin(), weight.end(), 0.0); + + /* make a normalized and accumulated version of the weight vector */ + const size_t m = weight.size(); + vector w; w.reserve(m); + for ( size_t i = 0 ; i < m ; ++i ) { + w.push_back(weight[i]/sum); + } + + vector acc(m); + std::partial_sum(w.begin(),w.end(),acc.begin()); + + /* iid sample n times */ + vector result; result.reserve(n); + const double denominator = (double)RAND_MAX; + for ( size_t i = 0 ; i < n ; ++i ) { + const double value = rand() / denominator; + /* binary search the interval containing "value" */ + vector::iterator it = std::lower_bound(acc.begin(), acc.end(), value); + size_t idx = it - acc.begin(); + result.push_back(idx); + } + return result; +} + +/*****************************************************************************/ +vector uniqueSampler(const vector &weight, const size_t n) { + + const size_t m = weight.size(); + if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size"); + + vector result; + + size_t count = 0; + std::vector touched(m, false); + while ( count < n ) { + std::vector localIndices; localIndices.reserve(n-count); + std::vector localWeights; localWeights.reserve(n-count); + + /* collect data */ + for ( size_t i = 0 ; i < m ; ++i ) { + if ( !touched[i] ) { + localIndices.push_back(i); + localWeights.push_back(weight[i]); } - result->push_back(jf); - } - return result; - } - - /* ************************************************************************* */ - SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, - const sharedBayesNet& Rc1, const sharedValues& xbar) : - Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))) { - } - - /* ************************************************************************* */ - // x = xbar + inv(R1)*y - VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { - return *xbar_ + Rc1_->backSubstitute(y); - } - - /* ************************************************************************* */ - double SubgraphPreconditioner::error(const VectorValues& y) const { - Errors e(y); - VectorValues x = this->x(y); - Errors e2 = Ab2()->gaussianErrors(x); - return 0.5 * (dot(e, e) + dot(e2,e2)); - } - - /* ************************************************************************* */ - // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), - VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const { - VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ - Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ - VectorValues v = VectorValues::Zero(x); - Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ - return y + Rc1()->backSubstituteTranspose(v); - } - - /* ************************************************************************* */ - // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] - Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { - - Errors e(y); - VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ - Errors e2 = *Ab2() * x; /* A2*x */ - e.splice(e.end(), e2); - return e; - } - - /* ************************************************************************* */ - // In-place version that overwrites e - void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { - - Errors::iterator ei = e.begin(); - for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) { - *ei = y[i]; } - // Add A2 contribution - VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y - Ab2()->multiplyInPlace(x, ei); // use iterator version - } - - /* ************************************************************************* */ - // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 - VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { - - Errors::const_iterator it = e.begin(); - VectorValues y = zero(); - for ( Key i = 0 ; i < y.size() ; ++i, ++it ) - y[i] = *it ; - transposeMultiplyAdd2(1.0,it,e.end(),y); - return y; - } - - /* ************************************************************************* */ - // y += alpha*A'*e - void SubgraphPreconditioner::transposeMultiplyAdd - (double alpha, const Errors& e, VectorValues& y) const { - - Errors::const_iterator it = e.begin(); - for ( Key i = 0 ; i < y.size() ; ++i, ++it ) { - const Vector& ei = *it; - axpy(alpha, ei, y[i]); + /* sampling and cache results */ + vector samples = iidSampler(localWeights, n-count); + BOOST_FOREACH ( const size_t &id, samples ) { + if ( touched[id] == false ) { + touched[id] = true ; + result.push_back(id); + if ( ++count >= n ) break; + } } - transposeMultiplyAdd2(alpha, it, e.end(), y); + } + return result; +} + +/****************************************************************************/ +Subgraph::Subgraph(const std::vector &indices) { + edges_.reserve(indices.size()); + BOOST_FOREACH ( const size_t &idx, indices ) { + edges_.push_back(SubgraphEdge(idx, 1.0)); + } +} + +/****************************************************************************/ +std::vector Subgraph::edgeIndices() const { + std::vector eid; eid.reserve(size()); + BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) { + eid.push_back(edge.index_); + } + return eid; +} + +/****************************************************************************/ +void Subgraph::save(const std::string &fn) const { + std::ofstream os(fn.c_str()); + boost::archive::text_oarchive oa(os); + oa << *this; + os.close(); +} + +/****************************************************************************/ +Subgraph::shared_ptr Subgraph::load(const std::string &fn) { + std::ifstream is(fn.c_str()); + boost::archive::text_iarchive ia(is); + Subgraph::shared_ptr subgraph(new Subgraph()); + ia >> *subgraph; + is.close(); + return subgraph; +} + +/****************************************************************************/ +std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) { + if ( edge.weight() != 1.0 ) + os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")"; + else + os << edge.index() ; + return os; +} + +/****************************************************************************/ +std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) { + os << "Subgraph" << endl; + BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) { + os << e << ", " ; + } + return os; +} + +/*****************************************************************************/ +void SubgraphBuilderParameters::print() const { + print(cout); +} + +/***************************************************************************************/ +void SubgraphBuilderParameters::print(ostream &os) const { + os << "SubgraphBuilderParameters" << endl + << "skeleton: " << skeletonTranslator(skeleton_) << endl + << "skeleton weight: " << skeletonWeightTranslator(skeletonWeight_) << endl + << "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl + ; +} + +/*****************************************************************************/ +ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) { + p.print(os); + return os; +} + +/*****************************************************************************/ +SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){ + std::string s = src; boost::algorithm::to_upper(s); + if (s == "NATURALCHAIN") return NATURALCHAIN; + else if (s == "BFS") return BFS; + else if (s == "KRUSKAL") return KRUSKAL; + throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s); + return KRUSKAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) { + if ( w == NATURALCHAIN )return "NATURALCHAIN"; + else if ( w == BFS ) return "BFS"; + else if ( w == KRUSKAL )return "KRUSKAL"; + else return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "EQUAL") return EQUAL; + else if (s == "RHS") return RHS_2NORM; + else if (s == "LHS") return LHS_FNORM; + else if (s == "RANDOM") return RANDOM; + throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s); + return EQUAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) { + if ( w == EQUAL ) return "EQUAL"; + else if ( w == RHS_2NORM ) return "RHS"; + else if ( w == LHS_FNORM ) return "LHS"; + else if ( w == RANDOM ) return "RANDOM"; + else return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "SKELETON") return SKELETON; +// else if (s == "STRETCH") return STRETCH; +// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH; + throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s); + return SKELETON; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) { + if ( w == SKELETON ) return "SKELETON"; +// else if ( w == STRETCH ) return "STRETCH"; +// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH"; + else return "UNKNOWN"; +} + +/****************************************************************/ +std::vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { + const SubgraphBuilderParameters &p = parameters_; + switch (p.skeleton_) { + case SubgraphBuilderParameters::NATURALCHAIN: + return natural_chain(gfg); + break; + case SubgraphBuilderParameters::BFS: + return bfs(gfg); + break; + case SubgraphBuilderParameters::KRUSKAL: + return kruskal(gfg, ordering, w); + break; + default: + cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; + break; + } + return vector(); +} + +/****************************************************************/ +std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { + std::vector result ; + size_t idx = 0; + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + if ( gf->size() == 1 ) { + result.push_back(idx); + } + idx++; + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { + std::vector result ; + size_t idx = 0; + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + if ( gf->size() == 2 ) { + const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; + if ( (k1-k0) == 1 || (k0-k1) == 1 ) + result.push_back(idx); + } + idx++; + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { + const VariableIndex variableIndex(gfg); + /* start from the first key of the first factor */ + Key seed = gfg[0]->keys()[0]; + + const size_t n = variableIndex.size(); + + /* each vertex has self as the predecessor */ + std::vector result; + result.reserve(n-1); + + /* Initialize */ + std::queue q; + q.push(seed); + + std::set flags; + flags.insert(seed); + + /* traversal */ + while ( !q.empty() ) { + const size_t head = q.front(); q.pop(); + BOOST_FOREACH ( const size_t id, variableIndex[head] ) { + const GaussianFactor &gf = *gfg[id]; + BOOST_FOREACH ( const size_t key, gf.keys() ) { + if ( flags.count(key) == 0 ) { + q.push(key); + flags.insert(key); + result.push_back(id); + } + } + } + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { + const VariableIndex variableIndex(gfg); + const size_t n = variableIndex.size(); + const vector idx = sort_idx(w) ; + + /* initialize buffer */ + vector result; + result.reserve(n-1); + + // container for acsendingly sorted edges + DSFVector D(n) ; + + size_t count = 0 ; double sum = 0.0 ; + BOOST_FOREACH (const size_t id, idx) { + const GaussianFactor &gf = *gfg[id]; + if ( gf.keys().size() != 2 ) continue; + const size_t u = ordering.find(gf.keys()[0])->second, + u_root = D.find(u), + v = ordering.find(gf.keys()[1])->second, + v_root = D.find(v) ; + if ( u_root != v_root ) { + D.merge(u_root, v_root) ; + result.push_back(id) ; + sum += w[id] ; + if ( ++count == n-1 ) break ; + } + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::sample(const std::vector &weights, const size_t t) const { + return uniqueSampler(weights, t); +} + +/****************************************************************/ +Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { + + const SubgraphBuilderParameters &p = parameters_; + const Ordering inverse_ordering = Ordering::Natural(gfg); + const FastMap forward_ordering = inverse_ordering.invert(); + const size_t n = inverse_ordering.size(), t = n * p.complexity_ ; + + vector w = weights(gfg); + const vector tree = buildTree(gfg, forward_ordering, w); + + /* sanity check */ + if ( tree.size() != n-1 ) { + throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed "); } - /* ************************************************************************* */ - // y += alpha*inv(R1')*A2'*e2 - void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, + /* down weight the tree edges to zero */ + BOOST_FOREACH ( const size_t id, tree ) { + w[id] = 0.0; + } + + /* decide how many edges to augment */ + std::vector offTree = sample(w, t); + + vector subgraph = unary(gfg); + subgraph.insert(subgraph.end(), tree.begin(), tree.end()); + subgraph.insert(subgraph.end(), offTree.begin(), offTree.end()); + + return boost::make_shared(subgraph); +} + +/****************************************************************/ +SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const +{ + const size_t m = gfg.size() ; + Weights weight; weight.reserve(m); + + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) { + switch ( parameters_.skeletonWeight_ ) { + case SubgraphBuilderParameters::EQUAL: + weight.push_back(1.0); + break; + case SubgraphBuilderParameters::RHS_2NORM: + { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(jf->getb().norm()); + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(hf->linearTerm().norm()); + } + } + break; + case SubgraphBuilderParameters::LHS_FNORM: + { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(std::sqrt(jf->getA().squaredNorm())); + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(std::sqrt(hf->information().squaredNorm())); + } + } + break; + + case SubgraphBuilderParameters::RANDOM: + weight.push_back(std::rand()%100 + 1.0); + break; + + default: + throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme "); + break; + } + } + return weight; +} + +/* ************************************************************************* */ +SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) : + parameters_(p) {} + +/* ************************************************************************* */ +SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, + const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) : + Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), + b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) { +} + +/* ************************************************************************* */ +// x = xbar + inv(R1)*y +VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { + return *xbar_ + Rc1_->backSubstitute(y); +} + +/* ************************************************************************* */ +double SubgraphPreconditioner::error(const VectorValues& y) const { + Errors e(y); + VectorValues x = this->x(y); + Errors e2 = Ab2()->gaussianErrors(x); + return 0.5 * (dot(e, e) + dot(e2,e2)); +} + +/* ************************************************************************* */ +// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), +VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const { + VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ + Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ + VectorValues v = VectorValues::Zero(x); + Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ + return y + Rc1()->backSubstituteTranspose(v); +} + +/* ************************************************************************* */ +// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] +Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { + + Errors e(y); + VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ + Errors e2 = *Ab2() * x; /* A2*x */ + e.splice(e.end(), e2); + return e; +} + +/* ************************************************************************* */ +// In-place version that overwrites e +void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { + + Errors::iterator ei = e.begin(); + for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) { + *ei = y[i]; + } + + // Add A2 contribution + VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y + Ab2()->multiplyInPlace(x, ei); // use iterator version +} + +/* ************************************************************************* */ +// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 +VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { + + Errors::const_iterator it = e.begin(); + VectorValues y = zero(); + for ( Key i = 0 ; i < y.size() ; ++i, ++it ) + y[i] = *it ; + transposeMultiplyAdd2(1.0,it,e.end(),y); + return y; +} + +/* ************************************************************************* */ +// y += alpha*A'*e +void SubgraphPreconditioner::transposeMultiplyAdd +(double alpha, const Errors& e, VectorValues& y) const { + + Errors::const_iterator it = e.begin(); + for ( Key i = 0 ; i < y.size() ; ++i, ++it ) { + const Vector& ei = *it; + axpy(alpha, ei, y[i]); + } + transposeMultiplyAdd2(alpha, it, e.end(), y); +} + +/* ************************************************************************* */ +// y += alpha*inv(R1')*A2'*e2 +void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const { - // create e2 with what's left of e - // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? - Errors e2; - while (it != end) e2.push_back(*(it++)); + // create e2 with what's left of e + // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? + Errors e2; + while (it != end) e2.push_back(*(it++)); - VectorValues x = VectorValues::Zero(y); // x = 0 - Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 - axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x + VectorValues x = VectorValues::Zero(y); // x = 0 + Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 + axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x +} + +/* ************************************************************************* */ +void SubgraphPreconditioner::print(const std::string& s) const { + cout << s << endl; + Ab2_->print(); +} + +/*****************************************************************************/ +void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const +{ + /* copy first */ + std::copy(y.data(), y.data() + y.rows(), x.data()); + + /* in place back substitute */ + BOOST_REVERSE_FOREACH(const boost::shared_ptr & cg, *Rc1_) { + /* collect a subvector of x that consists of the parents of cg (S) */ + const Vector xParent = getSubvector(x, keyInfo_, FastVector(cg->beginParents(), cg->endParents())); + const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals())); + + /* compute the solution for the current pivot */ + const Vector solFrontal = cg->get_R().triangularView().solve(rhsFrontal - cg->get_S() * xParent); + + /* assign subvector of sol to the frontal variables */ + setSubvector(solFrontal, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals()), x); + } +} + +/*****************************************************************************/ +void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const +{ + /* copy first */ + std::copy(y.data(), y.data() + y.rows(), x.data()); + + /* in place back substitute */ + BOOST_FOREACH(const boost::shared_ptr & cg, *Rc1_) { + const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals())); +// const Vector solFrontal = cg->get_R().triangularView().transpose().solve(rhsFrontal); + const Vector solFrontal = cg->get_R().transpose().triangularView().solve(rhsFrontal); + + // Check for indeterminant solution + if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front()); + + /* assign subvector of sol to the frontal variables */ + setSubvector(solFrontal, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals()), x); + + /* substract from parent variables */ + for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) { + KeyInfo::const_iterator it2 = keyInfo_.find(*it); + Eigen::Map rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1); + rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal; + } + } +} + +/*****************************************************************************/ +void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) +{ + /* identify the subgraph structure */ + const SubgraphBuilder builder(parameters_.builderParams_); + Subgraph::shared_ptr subgraph = builder(gfg); + + keyInfo_ = keyInfo; + + /* build factor subgraph */ + GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true); + + /* factorize and cache BayesNet */ + Rc1_ = gfg_subgraph->eliminateSequential(); +} + +/*****************************************************************************/ +Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys) { + + /* a cache of starting index and dim */ + typedef vector > Cache; + Cache cache; + + /* figure out dimension by traversing the keys */ + size_t d = 0; + BOOST_FOREACH ( const Key &key, keys ) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + cache.push_back(make_pair(entry.colstart(), entry.dim())); + d += entry.dim(); } - /* ************************************************************************* */ - void SubgraphPreconditioner::print(const std::string& s) const { - cout << s << endl; - Ab2_->print(); + /* use the cache to fill the result */ + Vector result = Vector::Zero(d, 1); + size_t idx = 0; + BOOST_FOREACH ( const Cache::value_type &p, cache ) { + result.segment(idx, p.second) = src.segment(p.first, p.second) ; + idx += p.second; } + + return result; +} + +/*****************************************************************************/ +void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys, Vector &dst) { + /* use the cache */ + size_t idx = 0; + BOOST_FOREACH ( const Key &key, keys ) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ; + idx += entry.dim(); + } +} + +/*****************************************************************************/ +boost::shared_ptr +buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) { + + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + result->reserve(subgraph.size()); + BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) { + const size_t idx = e.index(); + if ( clone ) result->push_back(gfg[idx]->clone()); + else result->push_back(gfg[idx]); + } + return result; +} + } // nsamespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index d9d7524b6..e237b5853 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -12,15 +12,16 @@ /** * @file SubgraphPreconditioner.h * @date Dec 31, 2009 - * @author Frank Dellaert + * @author Frank Dellaert, Yong-Dian Jian */ #pragma once #include -#include #include - +#include +#include +#include #include namespace gtsam { @@ -30,6 +31,146 @@ namespace gtsam { class GaussianFactorGraph; class VectorValues; + struct SubgraphEdge { + size_t index_; /* edge id */ + double weight_; /* edge weight */ + SubgraphEdge() : index_(0), weight_(1.0) {} + SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {} + SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {} + inline size_t index() const { return index_; } + inline double weight() const { return weight_; } + inline bool isUnitWeight() const { return (weight_ == 1.0); } + friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge); + private: + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(index_); + ar & BOOST_SERIALIZATION_NVP(weight_); + } + }; + + /**************************************************************************/ + class Subgraph { + public: + typedef boost::shared_ptr shared_ptr; + typedef std::vector vector_shared_ptr; + typedef std::vector Edges; + typedef std::vector EdgeIndices; + typedef Edges::iterator iterator; + typedef Edges::const_iterator const_iterator; + + protected: + Edges edges_; /* index to the factors */ + + public: + Subgraph() {} + Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {} + Subgraph(const Edges &edges) : edges_(edges) {} + Subgraph(const std::vector &indices) ; + + inline const Edges& edges() const { return edges_; } + inline const size_t size() const { return edges_.size(); } + EdgeIndices edgeIndices() const; + + iterator begin() { return edges_.begin(); } + const_iterator begin() const { return edges_.begin(); } + iterator end() { return edges_.end(); } + const_iterator end() const { return edges_.end(); } + + void save(const std::string &fn) const; + static shared_ptr load(const std::string &fn); + friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); + + private: + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(edges_); + } + }; + + /****************************************************************************/ + struct SubgraphBuilderParameters { + public: + typedef boost::shared_ptr shared_ptr; + + enum Skeleton { + /* augmented tree */ + NATURALCHAIN = 0, /* natural ordering of the graph */ + BFS, /* breadth-first search tree */ + KRUSKAL, /* maximum weighted spanning tree */ + } skeleton_ ; + + enum SkeletonWeight { /* how to weigh the graph edges */ + EQUAL = 0, /* every block edge has equal weight */ + RHS_2NORM, /* use the 2-norm of the rhs */ + LHS_FNORM, /* use the frobenius norm of the lhs */ + RANDOM, /* bounded random edge weight */ + } skeletonWeight_ ; + + enum AugmentationWeight { /* how to weigh the graph edges */ + SKELETON = 0, /* use the same weights in building the skeleton */ +// STRETCH, /* stretch in the laplacian sense */ +// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */ + } augmentationWeight_ ; + + double complexity_; + + SubgraphBuilderParameters() + : skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {} + virtual ~SubgraphBuilderParameters() {} + + /* for serialization */ + void print() const ; + virtual void print(std::ostream &os) const ; + friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p); + + static Skeleton skeletonTranslator(const std::string &s); + static std::string skeletonTranslator(Skeleton w); + static SkeletonWeight skeletonWeightTranslator(const std::string &s); + static std::string skeletonWeightTranslator(SkeletonWeight w); + static AugmentationWeight augmentationWeightTranslator(const std::string &s); + static std::string augmentationWeightTranslator(AugmentationWeight w); + }; + + /*****************************************************************************/ + class SubgraphBuilder { + + public: + typedef SubgraphBuilder Base; + typedef boost::shared_ptr shared_ptr; + typedef std::vector Weights; + + SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : parameters_(p) {} + virtual ~SubgraphBuilder() {} + virtual boost::shared_ptr operator() (const GaussianFactorGraph &jfg) const ; + + private: + std::vector buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &weights) const ; + std::vector unary(const GaussianFactorGraph &gfg) const ; + std::vector natural_chain(const GaussianFactorGraph &gfg) const ; + std::vector bfs(const GaussianFactorGraph &gfg) const ; + std::vector kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const ; + std::vector sample(const std::vector &weights, const size_t t) const ; + Weights weights(const GaussianFactorGraph &gfg) const; + SubgraphBuilderParameters parameters_; + + private: + SubgraphBuilder() {} + }; + + /*******************************************************************************************/ + struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters { + typedef PreconditionerParameters Base; + typedef boost::shared_ptr shared_ptr; + SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : Base(), builderParams_(p) {} + virtual ~SubgraphPreconditionerParameters() {} + SubgraphBuilderParameters builderParams_; + }; + /** * Subgraph conditioner class, as explained in the RSS 2010 submission. * Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2 @@ -37,7 +178,7 @@ namespace gtsam { * To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2. * Then solve for yhat using CG, and solve for xhat = system.x(yhat). */ - class GTSAM_EXPORT SubgraphPreconditioner { + class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner { public: typedef boost::shared_ptr shared_ptr; @@ -52,9 +193,12 @@ namespace gtsam { sharedValues xbar_; ///< A1 \ b1 sharedErrors b2bar_; ///< A2*xbar - b2 + KeyInfo keyInfo_; + SubgraphPreconditionerParameters parameters_; + public: - SubgraphPreconditioner(); + SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters()); /** * Constructor @@ -62,7 +206,10 @@ namespace gtsam { * @param Rc1: the Bayes Net R1*x=c1 * @param xbar: the solution to R1*x=c1 */ - SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); + SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar, + const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters()); + + virtual ~SubgraphPreconditioner() {} /** print the object */ void print(const std::string& s = "SubgraphPreconditioner") const; @@ -119,6 +266,54 @@ namespace gtsam { * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] */ void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const; + + /*****************************************************************************/ + /* implement virtual functions of Preconditioner */ + + /* Computation Interfaces for Vector */ + virtual void solve(const Vector& y, Vector &x) const; + virtual void transposeSolve(const Vector& y, Vector& x) const ; + + virtual void build( + const GaussianFactorGraph &gfg, + const KeyInfo &info, + const std::map &lambda + ) ; + /*****************************************************************************/ }; + /* get subvectors */ + Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys); + + /* set subvectors */ + void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys, Vector &dst); + + + /* build a factor subgraph, which is defined as a set of weighted edges (factors) */ + boost::shared_ptr + buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone); + + + /* sort the container and return permutation index with default comparator */ + template + std::vector sort_idx(const Container &src) + { + typedef typename Container::value_type T; + const size_t n = src.size() ; + std::vector > tmp; + tmp.reserve(n); + for ( size_t i = 0 ; i < n ; i++ ) + tmp.push_back(std::make_pair(i, src[i])); + + /* sort */ + std::stable_sort(tmp.begin(), tmp.end()) ; + + /* copy back */ + std::vector idx; idx.reserve(n); + for ( size_t i = 0 ; i < n ; i++ ) { + idx.push_back(tmp[i].first) ; + } + return idx; + } + } // namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 034fdeeaf..6f10d04ad 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -143,4 +144,11 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { return boost::tie(At, Ac); } +/****************************************************************************/ +VectorValues SubgraphSolver::optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial +) { return VectorValues(); } } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index a66182281..e4d3f50f3 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -13,7 +13,6 @@ #include -#include #include #include #include @@ -23,6 +22,7 @@ namespace gtsam { // Forward declarations class GaussianFactorGraph; class GaussianBayesNet; + class SubgraphPreconditioner; class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { public: @@ -61,7 +61,7 @@ public: protected: Parameters parameters_; Ordering ordering_; - SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object public: /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ @@ -87,7 +87,7 @@ public: const KeyInfo &keyInfo, const std::map &lambda, const VectorValues &initial - ) { return VectorValues(); } + ) ; protected: diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index f365fc524..5e8e3923c 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include class NonlinearOptimizerMoreOptimizationTest; diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 2a6eb5887..88bece4e1 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -103,6 +103,7 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, } else if (params.isSequential()) { delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); } else if (params.isIterative()) { + if (!params.iterativeParams) throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ..."); diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index ef74ad1ef..38a40521a 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -88,7 +89,6 @@ TEST( PCGSolver, llt ) { } - /* ************************************************************************* */ TEST( PCGSolver, dummy ) { @@ -129,6 +129,26 @@ TEST( PCGSolver, blockjacobi ) DOUBLES_EQUAL(0,fg.error(actualPCG),tol); } +/* ************************************************************************* */ +TEST( PCGSolver, subgraph ) +{ + LevenbergMarquardtParams paramsPCG; + paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; + PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->preconditioner_ = boost::make_shared(); + paramsPCG.iterativeParams = pcg; + + NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); + + Point2 x0(10,10); + Values c0; + c0.insert(X(1), x0); + + Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); + + DOUBLES_EQUAL(0,fg.error(actualPCG),tol); +} + /* ************************************************************************* */ int main() { TestResult tr; From 0a7fd40b6c89946966caba9bafc7b0573d7be840 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Tue, 17 Jun 2014 13:47:43 -0400 Subject: [PATCH 82/90] fix matlab toolbox compilation error --- gtsam.h | 4 ++-- gtsam/linear/ConjugateGradientSolver.h | 2 ++ gtsam/linear/SubgraphSolver.h | 1 + gtsam/nonlinear/NonlinearOptimizerParams.cpp | 4 ++-- gtsam/nonlinear/NonlinearOptimizerParams.h | 4 +++- 5 files changed, 10 insertions(+), 5 deletions(-) diff --git a/gtsam.h b/gtsam.h index 2cd6415a6..838c48efc 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1514,7 +1514,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { void print() const; }; -class SubgraphSolver { +virtual class SubgraphSolver { SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); gtsam::VectorValues optimize() const; @@ -1853,7 +1853,7 @@ virtual class NonlinearOptimizerParams { void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); - void setIterativeParams(const gtsam::SubgraphSolverParameters ¶ms); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); bool isMultifrontal() const; bool isSequential() const; diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 2382a0eb1..6e8509309 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -71,6 +71,8 @@ public: inline void setEpsilon_rel(double value) { epsilon_rel_ = value; } inline void setEpsilon_abs(double value) { epsilon_abs_ = value; } + + void print() const { Base::print(); } virtual void print(std::ostream &os) const; static std::string blasTranslator(const BLASKernel k) ; diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index e4d3f50f3..ac8a9da87 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -28,6 +28,7 @@ class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters public: typedef ConjugateGradientParameters Base; SubgraphSolverParameters() : Base() {} + void print() const { Base::print(); } virtual void print(std::ostream &os) const { Base::print(os); } }; diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index e0b6e0c6c..b7ad342ca 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -66,8 +66,8 @@ std::string NonlinearOptimizerParams::verbosityTranslator( /* ************************************************************************* */ void NonlinearOptimizerParams::setIterativeParams( - const SubgraphSolverParameters ¶ms) { - iterativeParams = boost::make_shared(params); + const boost::shared_ptr params) { + iterativeParams = params; } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 641c6da24..cd5496209 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -148,7 +148,9 @@ public: void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } - void setIterativeParams(const SubgraphSolverParameters& params); + + void setIterativeParams(const boost::shared_ptr params); + void setOrdering(const Ordering& ordering) { this->ordering = ordering; } From ea0dbf05ef919c343de91e2058309eb85a5107dd Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Jun 2014 14:36:09 -0400 Subject: [PATCH 83/90] Name change from k3,k4 to p1,p2 as in OpenCV --- .cproject | 2280 ++++++++++++++++---------------- gtsam/geometry/Cal3DS2.cpp | 84 +- gtsam/geometry/Cal3DS2.h | 47 +- gtsam/geometry/Cal3Unified.cpp | 2 +- gtsam/geometry/Cal3Unified.h | 19 +- 5 files changed, 1221 insertions(+), 1211 deletions(-) diff --git a/.cproject b/.cproject index d5a6ca4d4..1783efa98 100644 --- a/.cproject +++ b/.cproject @@ -568,7 +568,6 @@ make - tests/testBayesTree.run true false @@ -576,7 +575,6 @@ make - testBinaryBayesNet.run true false @@ -624,7 +622,6 @@ make - testSymbolicBayesNet.run true false @@ -632,7 +629,6 @@ make - tests/testSymbolicFactor.run true false @@ -640,7 +636,6 @@ make - testSymbolicFactorGraph.run true false @@ -656,12 +651,19 @@ make - tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -678,6 +680,142 @@ true true + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + testCal3Unified.run + true + true + true + make -j5 @@ -718,6 +856,62 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -734,6 +928,30 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + make -j2 @@ -806,6 +1024,46 @@ true true + + make + -j5 + testWrap.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -974,6 +1232,307 @@ true true + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + testJunctionTree.run + true + false + true + + + make + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + make -j5 @@ -1080,7 +1639,6 @@ make - testErrors.run true false @@ -1126,6 +1684,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -1310,6 +1876,54 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -1318,6 +1932,70 @@ true true + + make + -j5 + testAntiFactor.run + true + true + true + + + make + -j5 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + make -j5 @@ -1416,6 +2094,7 @@ make + testSimulated2DOriented.run true false @@ -1455,6 +2134,7 @@ make + testSimulated2D.run true false @@ -1462,6 +2142,7 @@ make + testSimulated3D.run true false @@ -1515,1140 +2196,6 @@ false true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testLieConfig.run - true - true - true - - - make - -j3 - install - true - false - true - - - make - -j2 - clean - true - true - true - - - make - -j1 - check - true - false - true - - - make - -j5 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j5 - gtsam-shared - true - true - true - - - make - -j5 - gtsam-static - true - true - true - - - make - -j5 - timing - true - true - true - - - make - -j5 - examples - true - true - true - - - make - -j5 - VERBOSE=1 all - true - true - true - - - make - -j5 - VERBOSE=1 check - true - true - true - - - make - -j5 - check.base - true - true - true - - - make - -j5 - timing.base - true - true - true - - - make - -j2 VERBOSE=1 - check.geometry - true - false - true - - - make - -j5 - timing.geometry - true - true - true - - - make - -j2 VERBOSE=1 - check.inference - true - false - true - - - make - -j5 - timing.inference - true - true - true - - - make - -j2 VERBOSE=1 - check.linear - true - false - true - - - make - -j5 - timing.linear - true - true - true - - - make - -j2 VERBOSE=1 - check.nonlinear - true - false - true - - - make - -j5 - timing.nonlinear - true - true - true - - - make - -j2 VERBOSE=1 - check.slam - true - false - true - - - make - -j5 - timing.slam - true - true - true - - - make - -j5 - wrap_gtsam - true - true - true - - - make - VERBOSE=1 - wrap_gtsam - true - false - true - - - cpack - - -G DEB - true - false - true - - - cpack - - -G RPM - true - false - true - - - cpack - - -G TGZ - true - false - true - - - cpack - - --config CPackSourceConfig.cmake - true - false - true - - - make - -j5 - check.discrete - true - true - true - - - make - -j5 - check.discrete_unstable - true - true - true - - - make - -j5 - check.base_unstable - true - true - true - - - make - -j5 - check.dynamics_unstable - true - true - true - - - make - -j5 - check.slam_unstable - true - true - true - - - make - -j5 - check.unstable - true - true - true - - - make - -j5 - wrap_gtsam_build - true - true - true - - - make - -j5 - wrap_gtsam_unstable_build - true - true - true - - - make - -j5 - wrap_gtsam_unstable - true - true - true - - - make - -j5 - wrap - true - true - true - - - make - -j5 - wrap_gtsam_distclean - true - true - true - - - make - -j5 - wrap_gtsam_unstable_distclean - true - true - true - - - make - -j5 - doc - true - true - true - - - make - -j5 - doc_clean - true - true - true - - - make - -j5 - check - true - true - true - - - make - -j5 - check.geometry_unstable - true - true - true - - - make - -j5 - check.linear_unstable - true - true - true - - - make - -j6 -j8 - gtsam_unstable-shared - true - true - true - - - make - -j6 -j8 - gtsam_unstable-static - true - true - true - - - make - -j6 -j8 - check.nonlinear_unstable - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - vSFMexample.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - - testJunctionTree.run - true - false - true - - - make - - testSymbolicBayesNetB.run - true - false - true - - - make - -j5 - testGaussianISAM.run - true - true - true - - - make - -j5 - testDoglegOptimizer.run - true - true - true - - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testAntiFactor.run - true - true - true - - - make - -j5 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - make -j2 @@ -2883,11 +2430,28 @@ make + tests/testGaussianISAM2 true false true + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testLieConfig.run + true + true + true + make -j2 @@ -2984,6 +2548,401 @@ true true + + make + -j3 + install + true + false + true + + + make + -j2 + clean + true + true + true + + + make + -j1 + check + true + false + true + + + make + -j5 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j5 + gtsam-shared + true + true + true + + + make + -j5 + gtsam-static + true + true + true + + + make + -j5 + timing + true + true + true + + + make + -j5 + examples + true + true + true + + + make + -j5 + VERBOSE=1 all + true + true + true + + + make + -j5 + VERBOSE=1 check + true + true + true + + + make + -j5 + check.base + true + true + true + + + make + -j5 + timing.base + true + true + true + + + make + -j2 VERBOSE=1 + check.geometry + true + false + true + + + make + -j5 + timing.geometry + true + true + true + + + make + -j2 VERBOSE=1 + check.inference + true + false + true + + + make + -j5 + timing.inference + true + true + true + + + make + -j2 VERBOSE=1 + check.linear + true + false + true + + + make + -j5 + timing.linear + true + true + true + + + make + -j2 VERBOSE=1 + check.nonlinear + true + false + true + + + make + -j5 + timing.nonlinear + true + true + true + + + make + -j2 VERBOSE=1 + check.slam + true + false + true + + + make + -j5 + timing.slam + true + true + true + + + make + -j5 + wrap_gtsam + true + true + true + + + make + VERBOSE=1 + wrap_gtsam + true + false + true + + + cpack + -G DEB + true + false + true + + + cpack + -G RPM + true + false + true + + + cpack + -G TGZ + true + false + true + + + cpack + --config CPackSourceConfig.cmake + true + false + true + + + make + -j5 + check.discrete + true + true + true + + + make + -j5 + check.discrete_unstable + true + true + true + + + make + -j5 + check.base_unstable + true + true + true + + + make + -j5 + check.dynamics_unstable + true + true + true + + + make + -j5 + check.slam_unstable + true + true + true + + + make + -j5 + check.unstable + true + true + true + + + make + -j5 + wrap_gtsam_build + true + true + true + + + make + -j5 + wrap_gtsam_unstable_build + true + true + true + + + make + -j5 + wrap_gtsam_unstable + true + true + true + + + make + -j5 + wrap + true + true + true + + + make + -j5 + wrap_gtsam_distclean + true + true + true + + + make + -j5 + wrap_gtsam_unstable_distclean + true + true + true + + + make + -j5 + doc + true + true + true + + + make + -j5 + doc_clean + true + true + true + + + make + -j5 + check + true + true + true + + + make + -j5 + check.geometry_unstable + true + true + true + + + make + -j5 + check.linear_unstable + true + true + true + + + make + -j6 -j8 + gtsam_unstable-shared + true + true + true + + + make + -j6 -j8 + gtsam_unstable-static + true + true + true + + + make + -j6 -j8 + check.nonlinear_unstable + true + true + true + + + make + -j5 + check.tests + true + true + true + make -j5 @@ -3008,6 +2967,45 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index e6c97cb1a..fa2495615 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -25,7 +25,7 @@ namespace gtsam { /* ************************************************************************* */ Cal3DS2::Cal3DS2(const Vector &v): - fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){} + fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} /* ************************************************************************* */ Matrix Cal3DS2::K() const { @@ -34,20 +34,20 @@ Matrix Cal3DS2::K() const { /* ************************************************************************* */ Vector Cal3DS2::vector() const { - return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); + return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_); } /* ************************************************************************* */ -void Cal3DS2::print(const std::string& s) const { - gtsam::print(K(), s + ".K"); - gtsam::print(Vector(k()), s + ".k"); +void Cal3DS2::print(const std::string& s_) const { + gtsam::print(K(), s_ + ".K"); + gtsam::print(Vector(k()), s_ + ".k"); } /* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || - fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol) + fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol) return false; return true; } @@ -57,10 +57,6 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { - // parameters - const double fx = fx_, fy = fy_, s = s_; - const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; - // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); // dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)]; @@ -68,40 +64,44 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y; const double rr = xx + yy; const double r4 = rr * rr; - const double g = 1. + k1 * rr + k2 * r4; - const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx); - const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy); + const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor + // tangential component + const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx); + const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy); + + // Radial and tangential distortion applied const double pnx = g*x + dx; const double pny = g*y + dy; // Inlined derivative for calibration if (H1) { - *H1 = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr, - fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy), - fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0, - fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy)); + *H1 = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx_ * x * rr + s_ * y * rr, + fx_ * x * r4 + s_ * y * r4, fx_ * 2. * xy + s_ * (rr + 2. * yy), + fx_ * (rr + 2. * xx) + s_ * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0, + fy_ * y * rr, fy_ * y * r4, fy_ * (rr + 2. * yy), fy_ * (2. * xy)); } // Inlined derivative for points if (H2) { const double dr_dx = 2. * x; const double dr_dy = 2. * y; - const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx; - const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy; + const double dg_dx = k1_ * dr_dx + k2_ * 2. * rr * dr_dx; + const double dg_dy = k1_ * dr_dy + k2_ * 2. * rr * dr_dy; - const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x); - const double dDx_dy = 2. * k3 * x + k4 * dr_dy; - const double dDy_dx = 2. * k4 * y + k3 * dr_dx; - const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y); + const double dDx_dx = 2. * p1_ * y + p2_ * (dr_dx + 4. * x); + const double dDx_dy = 2. * p1_ * x + p2_ * dr_dy; + const double dDy_dx = 2. * p2_ * y + p1_ * dr_dx; + const double dDy_dy = 2. * p2_ * x + p1_ * (dr_dy + 4. * y); - Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy); + Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_); Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy, y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy); *H2 = DK * DR; } - return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_); + // Regular uncalibrate after distortion + return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } /* ************************************************************************* */ @@ -123,8 +123,8 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y; const double rr = xx + yy; const double g = (1+k1_*rr+k2_*rr*rr); - const double dx = 2*k3_*xy + k4_*(rr+2*xx); - const double dy = 2*k4_*xy + k3_*(rr+2*yy); + const double dx = 2*p1_*xy + p2_*(rr+2*xx); + const double dy = 2*p2_*xy + p1_*(rr+2*yy); pn = (invKPi - Point2(dx,dy))/g; } @@ -136,24 +136,21 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { /* ************************************************************************* */ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { - //const double fx = fx_, fy = fy_, s = s_; - const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; - //const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y; const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; const double rr = xx + yy; const double dr_dx = 2*x; const double dr_dy = 2*y; const double r4 = rr*rr; - const double g = 1 + k1*rr + k2*r4; - const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx; - const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy; + const double g = 1 + k1_*rr + k2_*r4; + const double dg_dx = k1_*dr_dx + k2_*2*rr*dr_dx; + const double dg_dy = k1_*dr_dy + k2_*2*rr*dr_dy; - // Dx = 2*k3*xy + k4*(rr+2*xx); - // Dy = 2*k4*xy + k3*(rr+2*yy); - const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x); - const double dDx_dy = 2*k3*x + k4*dr_dy; - const double dDy_dx = 2*k4*y + k3*dr_dx; - const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y); + // Dx = 2*p1_*xy + p2_*(rr+2*xx); + // Dy = 2*p2_*xy + p1_*(rr+2*yy); + const double dDx_dx = 2*p1_*y + p2_*(dr_dx + 4*x); + const double dDx_dy = 2*p1_*x + p2_*dr_dy; + const double dDy_dx = 2*p2_*y + p1_*dr_dx; + const double dDy_dy = 2*p2_*x + p1_*(dr_dy + 4*y); Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_); Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy, @@ -167,16 +164,15 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y; const double rr = xx + yy; const double r4 = rr*rr; - const double fx = fx_, fy = fy_, s = s_; const double g = (1+k1_*rr+k2_*r4); - const double dx = 2*k3_*xy + k4_*(rr+2*xx); - const double dy = 2*k4_*xy + k3_*(rr+2*yy); + const double dx = 2*p1_*xy + p2_*(rr+2*xx); + const double dy = 2*p2_*xy + p1_*(rr+2*yy); const double pnx = g*x + dx; const double pny = g*y + dy; return (Matrix(2, 9) << - pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy), - 0.0, pny, 0.0, 0.0, 1.0, fy*y*rr , fy*y*r4 , fy*(rr+2*yy) , fy*(2*xy) ); + pnx, 0.0, pny, 1.0, 0.0, fx_*x*rr + s_*y*rr, fx_*x*r4 + s_*y*r4, fx_*2*xy + s_*(rr+2*yy), fx_*(rr+2*xx) + s_*(2*xy), + 0.0, pny, 0.0, 0.0, 1.0, fy_*y*rr , fy_*y*r4 , fy_*(rr+2*yy) , fy_*(2*xy) ); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index ced05086b..f796b708b 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -27,6 +27,15 @@ namespace gtsam { * @brief Calibration of a camera with radial distortion * @addtogroup geometry * \nosubgrouping + * + * Uses same distortionmodel as OpenCV, with + * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html + * but using only k1,k2,p1, and p2 coefficients. + * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + * rr = Pn.x^2 + Pn.y^2 + * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; + * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + * pi = K*pn */ class GTSAM_EXPORT Cal3DS2 : public DerivedValue { @@ -34,28 +43,22 @@ protected: double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double k1_, k2_ ; // radial 2nd-order and 4th-order - double k3_, k4_ ; // tangential distortion - - // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] - // rr = Pn.x^2 + Pn.y^2 - // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; - // k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] - // pi = K*pn + double p1_, p2_ ; // tangential distortion public: Matrix K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); } + Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } Vector vector() const ; /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {} Cal3DS2(double fx, double fy, double s, double u0, double v0, - double k1, double k2, double k3 = 0.0, double k4 = 0.0) : - fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} + double k1, double k2, double p1 = 0.0, double p2 = 0.0) : + fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} /// @} /// @name Advanced Constructors @@ -92,18 +95,30 @@ public: /// image center in y inline double py() const { return v0_;} + /// image center in x + inline double k1() const { return k1_;} + + /// image center in y + inline double k2() const { return k2_;} + + /// image center in x + inline double p1() const { return p1_;} + + /// image center in y + inline double p2() const { return p2_;} + /** - * convert intrinsic coordinates xy to image coordinates uv + * convert intrinsic coordinates xy to (distorted) image coordinates uv * @param p point in intrinsic coordinates * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in image coordinates + * @return point in (distorted) image coordinates */ Point2 uncalibrate(const Point2& p, boost::optional Dcal = boost::none, boost::optional Dp = boost::none) const ; - /// Conver a pixel coordinate to ideal coordinate + /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; /// Derivative of uncalibrate wrpt intrinsic coordinates @@ -148,8 +163,8 @@ private: ar & BOOST_SERIALIZATION_NVP(v0_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); - ar & BOOST_SERIALIZATION_NVP(k3_); - ar & BOOST_SERIALIZATION_NVP(k4_); + ar & BOOST_SERIALIZATION_NVP(p1_); + ar & BOOST_SERIALIZATION_NVP(p2_); } diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 808cb84a4..e7b408982 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -43,7 +43,7 @@ void Cal3Unified::print(const std::string& s) const { bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || - fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol || + fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol || fabs(xi_ - K.xi_) > tol) return false; return true; diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index a1d47332a..58e024c27 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -31,6 +31,14 @@ namespace gtsam { * @brief Calibration of a omni-directional camera with mirror + lens radial distortion * @addtogroup geometry * \nosubgrouping + * + * Similar to Cal3DS2, does distortion but has additional mirror parameter xi + * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + * Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] + * rr = Pn.x^2 + Pn.y^2 + * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; + * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + * pi = K*pn */ class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { @@ -41,13 +49,6 @@ private: double xi_; // mirror parameter - // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] - // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] - // rr = Pn.x^2 + Pn.y^2 - // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; - // k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] - // pi = K*pn - public: //Matrix K() const ; //Eigen::Vector4d k() const { return Base::k(); } @@ -60,8 +61,8 @@ public: Cal3Unified() : Base(), xi_(0) {} Cal3Unified(double fx, double fy, double s, double u0, double v0, - double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) : - Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} + double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) : + Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {} /// @} /// @name Advanced Constructors From 6ecc32a311050684a0935cf1428e8e948ee06a88 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Jun 2014 14:38:47 -0400 Subject: [PATCH 84/90] Fixed comments --- gtsam/geometry/Cal3DS2.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index f796b708b..51fe958d6 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -95,16 +95,16 @@ public: /// image center in y inline double py() const { return v0_;} - /// image center in x + /// First distortion coefficient inline double k1() const { return k1_;} - /// image center in y + /// Second distortion coefficient inline double k2() const { return k2_;} - /// image center in x + /// First tangential distortion coefficient inline double p1() const { return p1_;} - /// image center in y + /// Second tangential distortion coefficient inline double p2() const { return p2_;} /** From 0e64c9495e600a43daf7907feec0d73800d6dec7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Jun 2014 09:46:55 -0400 Subject: [PATCH 85/90] Two static functions save on copy/paste --- gtsam/geometry/Cal3DS2.cpp | 121 +++++++++++++-------------- gtsam/geometry/tests/testCal3DS2.cpp | 4 + 2 files changed, 63 insertions(+), 62 deletions(-) diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index fa2495615..d453e0c8f 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -52,6 +52,42 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { return true; } +/* ************************************************************************* */ +static Matrix D2dcalibration(double x, double y, double xx, double yy, + double xy, double rr, double r4, double fx, double fy, double s, double pnx, + double pny) { + return (Matrix(2, 9) << // + pnx, 0.0, pny, 1.0, 0.0, // + fx * x * rr + s * y * rr, fx * x * r4 + s * y * r4, // + fx * 2 * xy + s * (rr + 2 * yy), fx * (rr + 2 * xx) + s * (2 * xy), // + 0.0, pny, 0.0, 0.0, 1.0, // + fy * y * rr, fy * y * r4, // + fy * (rr + 2 * yy), fy * (2 * xy)); +} + +/* ************************************************************************* */ +static Matrix D2dintrinsic(double x, double y, double rr, double r4, double fx, + double fy, double s, double k1, double k2, double p1, double p2) { + const double drdx = 2 * x; + const double drdy = 2 * y; + const double g = 1 + k1 * rr + k2 * r4; + const double dgdx = k1 * drdx + k2 * 2 * rr * drdx; + const double dgdy = k1 * drdy + k2 * 2 * rr * drdy; + + // Dx = 2*p1*xy + p2*(rr+2*xx); + // Dy = 2*p2*xy + p1*(rr+2*yy); + const double dDxdx = 2 * p1 * y + p2 * (drdx + 4 * x); + const double dDxdy = 2 * p1 * x + p2 * drdy; + const double dDydx = 2 * p2 * y + p1 * drdx; + const double dDydy = 2 * p2 * x + p1 * (drdy + 4 * y); + + Matrix DK = (Matrix(2, 2) << fx, s, 0.0, fy); + Matrix DR = (Matrix(2, 2) << g + x * dgdx + dDxdx, x * dgdy + dDxdy, y * dgdx + + dDydx, g + y * dgdy + dDydy); + + return DK * DR; +} + /* ************************************************************************* */ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, @@ -74,31 +110,13 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, const double pnx = g*x + dx; const double pny = g*y + dy; - // Inlined derivative for calibration - if (H1) { - *H1 = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx_ * x * rr + s_ * y * rr, - fx_ * x * r4 + s_ * y * r4, fx_ * 2. * xy + s_ * (rr + 2. * yy), - fx_ * (rr + 2. * xx) + s_ * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0, - fy_ * y * rr, fy_ * y * r4, fy_ * (rr + 2. * yy), fy_ * (2. * xy)); - } - // Inlined derivative for points - if (H2) { - const double dr_dx = 2. * x; - const double dr_dy = 2. * y; - const double dg_dx = k1_ * dr_dx + k2_ * 2. * rr * dr_dx; - const double dg_dy = k1_ * dr_dy + k2_ * 2. * rr * dr_dy; + // Derivative for calibration + if (H1) + *H1 = D2dcalibration(x,y,xx,yy,xy,rr,r4,fx_,fy_,s_,pnx,pny); - const double dDx_dx = 2. * p1_ * y + p2_ * (dr_dx + 4. * x); - const double dDx_dy = 2. * p1_ * x + p2_ * dr_dy; - const double dDy_dx = 2. * p2_ * y + p1_ * dr_dx; - const double dDy_dy = 2. * p2_ * x + p1_ * (dr_dy + 4. * y); - - Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_); - Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy, - y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy); - - *H2 = DK * DR; - } + // Derivative for points + if (H2) + *H2 = D2dintrinsic(x, y, rr, r4, fx_, fy_, s_, k1_, k2_, p1_, p2_); // Regular uncalibrate after distortion return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); @@ -118,14 +136,14 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { // iterate until the uncalibrate is close to the actual pixel coordinate const int maxIterations = 10; int iteration; - for ( iteration = 0; iteration < maxIterations; ++iteration ) { - if ( uncalibrate(pn).distance(pi) <= tol ) break; - const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y; + for (iteration = 0; iteration < maxIterations; ++iteration) { + if (uncalibrate(pn).distance(pi) <= tol) break; + const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; const double rr = xx + yy; - const double g = (1+k1_*rr+k2_*rr*rr); - const double dx = 2*p1_*xy + p2_*(rr+2*xx); - const double dy = 2*p2_*xy + p1_*(rr+2*yy); - pn = (invKPi - Point2(dx,dy))/g; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); + pn = (invKPi - Point2(dx, dy)) / g; } if ( iteration >= maxIterations ) @@ -136,43 +154,22 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { /* ************************************************************************* */ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; const double rr = xx + yy; - const double dr_dx = 2*x; - const double dr_dy = 2*y; - const double r4 = rr*rr; - const double g = 1 + k1_*rr + k2_*r4; - const double dg_dx = k1_*dr_dx + k2_*2*rr*dr_dx; - const double dg_dy = k1_*dr_dy + k2_*2*rr*dr_dy; - - // Dx = 2*p1_*xy + p2_*(rr+2*xx); - // Dy = 2*p2_*xy + p1_*(rr+2*yy); - const double dDx_dx = 2*p1_*y + p2_*(dr_dx + 4*x); - const double dDx_dy = 2*p1_*x + p2_*dr_dy; - const double dDy_dx = 2*p2_*y + p1_*dr_dx; - const double dDy_dy = 2*p2_*x + p1_*(dr_dy + 4*y); - - Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_); - Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy, - y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy); - - return DK * DR; + return D2dintrinsic(x, y, rr, rr * rr, fx_, fy_, s_, k1_, k2_, p1_, p2_); } /* ************************************************************************* */ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y; + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; const double rr = xx + yy; - const double r4 = rr*rr; - const double g = (1+k1_*rr+k2_*r4); - const double dx = 2*p1_*xy + p2_*(rr+2*xx); - const double dy = 2*p2_*xy + p1_*(rr+2*yy); - const double pnx = g*x + dx; - const double pny = g*y + dy; - - return (Matrix(2, 9) << - pnx, 0.0, pny, 1.0, 0.0, fx_*x*rr + s_*y*rr, fx_*x*r4 + s_*y*r4, fx_*2*xy + s_*(rr+2*yy), fx_*(rr+2*xx) + s_*(2*xy), - 0.0, pny, 0.0, 0.0, 1.0, fy_*y*rr , fy_*y*r4 , fy_*(rr+2*yy) , fy_*(2*xy) ); + const double r4 = rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4); + const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); + const double pnx = g * x + dx; + const double pny = g * y + dy; + return D2dcalibration(x, y, xx, yy, xy, rr, r4, fx_, fy_, s_, pnx, pny); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index fc457767e..eddd3a674 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -60,6 +60,8 @@ TEST( Cal3DS2, Duncalibrate1) K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical,computed,1e-5)); + Matrix separate = K.D2d_calibration(p); + CHECK(assert_equal(numerical,separate,1e-5)); } /* ************************************************************************* */ @@ -68,6 +70,8 @@ TEST( Cal3DS2, Duncalibrate2) Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical,computed,1e-5)); + Matrix separate = K.D2d_intrinsic(p); + CHECK(assert_equal(numerical,separate,1e-5)); } /* ************************************************************************* */ From 12897a2c2936387c960eaa6e503cc4fb3ad3db8f Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Jun 2014 11:58:08 -0400 Subject: [PATCH 86/90] A small saving --- gtsam/geometry/Cal3DS2.cpp | 41 +++++++++++++++++++------------------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index d453e0c8f..d9d9642c0 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -66,32 +66,31 @@ static Matrix D2dcalibration(double x, double y, double xx, double yy, } /* ************************************************************************* */ -static Matrix D2dintrinsic(double x, double y, double rr, double r4, double fx, +static Matrix D2dintrinsic(double x, double y, double rr, double g, double fx, double fy, double s, double k1, double k2, double p1, double p2) { - const double drdx = 2 * x; - const double drdy = 2 * y; - const double g = 1 + k1 * rr + k2 * r4; - const double dgdx = k1 * drdx + k2 * 2 * rr * drdx; - const double dgdy = k1 * drdy + k2 * 2 * rr * drdy; + const double drdx = 2. * x; + const double drdy = 2. * y; + const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; + const double dgdy = k1 * drdy + k2 * 2. * rr * drdy; // Dx = 2*p1*xy + p2*(rr+2*xx); // Dy = 2*p2*xy + p1*(rr+2*yy); - const double dDxdx = 2 * p1 * y + p2 * (drdx + 4 * x); - const double dDxdy = 2 * p1 * x + p2 * drdy; - const double dDydx = 2 * p2 * y + p1 * drdx; - const double dDydy = 2 * p2 * x + p1 * (drdy + 4 * y); + const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x); + const double dDxdy = 2. * p1 * x + p2 * drdy; + const double dDydx = 2. * p2 * y + p1 * drdx; + const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); Matrix DK = (Matrix(2, 2) << fx, s, 0.0, fy); - Matrix DR = (Matrix(2, 2) << g + x * dgdx + dDxdx, x * dgdy + dDxdy, y * dgdx - + dDydx, g + y * dgdy + dDydy); + Matrix DR = (Matrix(2, 2) << // + g + x * dgdx + dDxdx, x * dgdy + dDxdy, // + y * dgdx + dDydx, g + y * dgdy + dDydy); return DK * DR; } /* ************************************************************************* */ -Point2 Cal3DS2::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { +Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, + boost::optional H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); @@ -107,16 +106,16 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy); // Radial and tangential distortion applied - const double pnx = g*x + dx; - const double pny = g*y + dy; + const double pnx = g * x + dx; + const double pny = g * y + dy; // Derivative for calibration if (H1) - *H1 = D2dcalibration(x,y,xx,yy,xy,rr,r4,fx_,fy_,s_,pnx,pny); + *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, fx_, fy_, s_, pnx, pny); // Derivative for points if (H2) - *H2 = D2dintrinsic(x, y, rr, r4, fx_, fy_, s_, k1_, k2_, p1_, p2_); + *H2 = D2dintrinsic(x, y, rr, g, fx_, fy_, s_, k1_, k2_, p1_, p2_); // Regular uncalibrate after distortion return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); @@ -156,7 +155,9 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; const double rr = xx + yy; - return D2dintrinsic(x, y, rr, rr * rr, fx_, fy_, s_, k1_, k2_, p1_, p2_); + const double r4 = rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4); + return D2dintrinsic(x, y, rr, g, fx_, fy_, s_, k1_, k2_, p1_, p2_); } /* ************************************************************************* */ From 8ce541f05f0da73f9cc260cdec46729d7244978f Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 19 Jun 2014 12:19:02 -0400 Subject: [PATCH 87/90] Separated out DK component --- gtsam/geometry/Cal3DS2.cpp | 48 ++++++++++++++++++++++---------------- 1 file changed, 28 insertions(+), 20 deletions(-) diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index d9d9642c0..c75eff022 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -53,21 +53,23 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { } /* ************************************************************************* */ -static Matrix D2dcalibration(double x, double y, double xx, double yy, - double xy, double rr, double r4, double fx, double fy, double s, double pnx, - double pny) { - return (Matrix(2, 9) << // - pnx, 0.0, pny, 1.0, 0.0, // - fx * x * rr + s * y * rr, fx * x * r4 + s * y * r4, // - fx * 2 * xy + s * (rr + 2 * yy), fx * (rr + 2 * xx) + s * (2 * xy), // - 0.0, pny, 0.0, 0.0, 1.0, // - fy * y * rr, fy * y * r4, // - fy * (rr + 2 * yy), fy * (2 * xy)); +static Eigen::Matrix D2dcalibration(double x, double y, double xx, + double yy, double xy, double rr, double r4, double pnx, double pny, + const Eigen::Matrix& DK) { + Eigen::Matrix DR1; + DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; + Eigen::Matrix DR2; + DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // + y * rr, y * r4, rr + 2 * yy, 2 * xy; + Eigen::Matrix D; + D << DR1, DK * DR2; + return D; } /* ************************************************************************* */ -static Matrix D2dintrinsic(double x, double y, double rr, double g, double fx, - double fy, double s, double k1, double k2, double p1, double p2) { +static Eigen::Matrix D2dintrinsic(double x, double y, double rr, + double g, double k1, double k2, double p1, double p2, + const Eigen::Matrix& DK) { const double drdx = 2. * x; const double drdy = 2. * y; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; @@ -80,10 +82,9 @@ static Matrix D2dintrinsic(double x, double y, double rr, double g, double fx, const double dDydx = 2. * p2 * y + p1 * drdx; const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); - Matrix DK = (Matrix(2, 2) << fx, s, 0.0, fy); - Matrix DR = (Matrix(2, 2) << // - g + x * dgdx + dDxdx, x * dgdy + dDxdy, // - y * dgdx + dDydx, g + y * dgdy + dDydy); + Eigen::Matrix DR; + DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // + y * dgdx + dDydx, g + y * dgdy + dDydy; return DK * DR; } @@ -109,13 +110,16 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, const double pnx = g * x + dx; const double pny = g * y + dy; + Eigen::Matrix DK; + if (H1 || H2) DK << fx_, s_, 0.0, fy_; + // Derivative for calibration if (H1) - *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, fx_, fy_, s_, pnx, pny); + *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); // Derivative for points if (H2) - *H2 = D2dintrinsic(x, y, rr, g, fx_, fy_, s_, k1_, k2_, p1_, p2_); + *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); // Regular uncalibrate after distortion return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); @@ -157,7 +161,9 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { const double rr = xx + yy; const double r4 = rr * rr; const double g = (1 + k1_ * rr + k2_ * r4); - return D2dintrinsic(x, y, rr, g, fx_, fy_, s_, k1_, k2_, p1_, p2_); + Eigen::Matrix DK; + DK << fx_, s_, 0.0, fy_; + return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); } /* ************************************************************************* */ @@ -170,7 +176,9 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); const double pnx = g * x + dx; const double pny = g * y + dy; - return D2dcalibration(x, y, xx, yy, xy, rr, r4, fx_, fy_, s_, pnx, pny); + Eigen::Matrix DK; + DK << fx_, s_, 0.0, fy_; + return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); } /* ************************************************************************* */ From de63d1981cea4165c5677556091c1e0d7d67730f Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 20 Jun 2014 11:57:48 -0400 Subject: [PATCH 88/90] Windows: Fix for discarding const qualifiers --- gtsam/linear/GaussianFactorGraph.cpp | 2 +- gtsam/linear/IterativeSolver.cpp | 2 +- gtsam/linear/SubgraphPreconditioner.cpp | 2 +- gtsam/linear/SubgraphPreconditioner.h | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index f10309d76..438a06783 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -61,7 +61,7 @@ namespace gtsam { for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) { map::iterator it2 = spec.find(*it); if ( it2 == spec.end() ) { - spec.insert(make_pair(*it, gf->getDim(it))); + spec.insert(make_pair(*it, gf->getDim(it))); } } } diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 36178b191..ab3ccde13 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -99,7 +99,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg){ for ( size_t i = 0 ; i < n ; ++i ) { const size_t key = ordering_[i]; const size_t dim = colspec.find(key)->second; - insert(make_pair(key, KeyInfoEntry(i, dim, start))); + insert(make_pair(key, KeyInfoEntry(i, dim, start))); start += dim ; } numCols_ = start; diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 20b434b9f..f5ee4ddfc 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -617,7 +617,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector< size_t d = 0; BOOST_FOREACH ( const Key &key, keys ) { const KeyInfoEntry &entry = keyInfo.find(key)->second; - cache.push_back(make_pair(entry.colstart(), entry.dim())); + cache.push_back(make_pair(entry.colstart(), entry.dim())); d += entry.dim(); } diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 261e435dc..4c65d14ca 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -302,7 +302,7 @@ namespace gtsam { std::vector > tmp; tmp.reserve(n); for ( size_t i = 0 ; i < n ; i++ ) - tmp.push_back(std::make_pair(i, src[i])); + tmp.push_back(std::make_pair(i, src[i])); /* sort */ std::stable_sort(tmp.begin(), tmp.end()) ; From a7cd94c9dc1323c1126804c57c99f9316f46a43f Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 20 Jun 2014 12:07:28 -0400 Subject: [PATCH 89/90] Fix struct vs. class mismatch warning --- gtsam/linear/PCGSolver.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index 8d48da149..0201000ad 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -22,7 +22,7 @@ namespace gtsam { class GaussianFactorGraph; class KeyInfo; class Preconditioner; -class PreconditionerParameters; +struct PreconditionerParameters; /*****************************************************************************/ struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { From 3bacdbbec5059a712eac4f79a1bf9a536f4daef7 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Fri, 20 Jun 2014 13:41:51 -0400 Subject: [PATCH 90/90] Move Smart Projection Factor wrapper to stable --- gtsam.h | 19 +++++++++++++++++++ gtsam_unstable/gtsam_unstable.h | 19 ------------------- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/gtsam.h b/gtsam.h index 0c00047b1..678e2a3d6 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2197,6 +2197,25 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { void serialize() const; }; +#include +template +virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { + + SmartProjectionPoseFactor(double rankTol, double linThreshold, + bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + + SmartProjectionPoseFactor(double rankTol); + SmartProjectionPoseFactor(); + + void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, + const CALIBRATION* K_i); + + // enabling serialization functionality + //void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; + #include template diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 256f313ac..06a41f5ec 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -380,25 +380,6 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; -#include -template -virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { - - SmartProjectionPoseFactor(double rankTol, double linThreshold, - bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); - - SmartProjectionPoseFactor(double rankTol); - SmartProjectionPoseFactor(); - - void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, - const CALIBRATION* K_i); - - // enabling serialization functionality - //void serialize() const; -}; - -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - #include template virtual class RangeFactor : gtsam::NonlinearFactor {