├── .gitignore ├── LICENSE ├── PostProcess_Matlab ├── AttUpdate.m ├── LongRunData.mat ├── LongRunInit.m ├── MAINRun.m ├── PosUpdate.m ├── Readme.md ├── VelUpdate.m ├── backwardSmoothing.m ├── circle.m ├── dcm2eulr.m ├── dema.m ├── ellipse3D.m ├── ellipsegen.m ├── error_ellipse.m ├── eulr2dcm.m ├── figGen.m ├── figureGeneration.m ├── geocentricLattitude.m ├── geocradius.m ├── getQins.m ├── gpsconversion.m ├── gravity.m ├── insErrorStateModel_LNF.m ├── llh2xyz.m ├── nonHolonomic.m ├── odom.m ├── odomMeasGen.m ├── odomUpt.m ├── odomslip.m ├── radicurv.m ├── skewsymm.m ├── smoothback.m ├── wheelTerrainContactAngle.m ├── xyz2enu.m ├── xyz2llh.m └── zeroUpd.m ├── README.md ├── core_navigation ├── CMakeLists.txt ├── config │ ├── init_params.yaml │ └── parameters.yaml ├── include │ └── core_navigation │ │ ├── CoreNav.h │ │ ├── InsConst.h │ │ └── InsUtils.h ├── launch │ └── test_online.launch ├── package.xml └── src │ ├── CoreNav.cpp │ ├── InsUtils.cpp │ └── core_nav.cpp ├── core_navigation_demos ├── doc │ ├── adam2.eps │ ├── adam2.jpg │ ├── architecturev2.jpg │ ├── architecturev2.png │ ├── corenav4.gif │ ├── covLong.jpg │ └── readme.md └── readme.md ├── geometry_utils ├── CMakeLists.txt ├── LICENSE ├── cmake │ └── FindEigen3.cmake ├── include │ └── geometry_utils │ │ ├── GeometryUtils.h │ │ ├── GeometryUtilsMath.h │ │ ├── GeometryUtilsROS.h │ │ ├── GeometryUtilsSerialization.h │ │ ├── Matrix2x2.h │ │ ├── Matrix3x3.h │ │ ├── Matrix4x4.h │ │ ├── MatrixNxMBase.h │ │ ├── MatrixNxNBase.h │ │ ├── Quaternion.h │ │ ├── Rotation2.h │ │ ├── Rotation3.h │ │ ├── RotationNBase.h │ │ ├── Transform2.h │ │ ├── Transform3.h │ │ ├── Vector2.h │ │ ├── Vector3.h │ │ ├── Vector4.h │ │ ├── VectorNBase.h │ │ └── YAMLLoader.h ├── package.xml └── tests │ ├── test_base.cc │ ├── test_equals.cc │ ├── test_math.cc │ └── test_so3error.cc └── parameter_utils ├── CMakeLists.txt ├── LICENSE ├── include └── parameter_utils │ └── ParameterUtils.h └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Cagri Kilic 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /PostProcess_Matlab/AttUpdate.m: -------------------------------------------------------------------------------- 1 | function [insAttitudePlus,Cb2nPlus,Cb2nMinus,Omega_n_en,Omega_n_ie,R_N,R_E,omega_n_in,omega_n_ie] = AttUpdate(insAttMinus,omega_ie,insLLHMinus,omega_b_ib,ecc,Ro,insVelMinus,dt) 2 | % insAttitudeMinus; 3 | % CbnMinus = eulr2dcm(insAttitudeMinus)'; % notice the tranpose 4 | Cn2bMinus = eulr2dcm(insAttMinus); %outputs Nav2Body 5 | Cb2nMinus=Cn2bMinus'; 6 | 7 | % eq 2.123 8 | omega_n_ie=[omega_ie*cos(insLLHMinus(1)); 9 | 0; 10 | (-1.0)*omega_ie*sin(insLLHMinus(1))]; 11 | % get body-rate with respect to interial 12 | % omega_b_ib= alpha_b_ib/dt; 13 | % skew-symmetric matrix (eq. 5.8)s 14 | Omega_b_ib= [0 -omega_b_ib(3) omega_b_ib(2); 15 | omega_b_ib(3) 0 -omega_b_ib(1); 16 | -omega_b_ib(2) omega_b_ib(1) 0]; 17 | 18 | % define the Earth's rotation vector represented in the local 19 | % navigation frame axes (eq. 5.41) 20 | Omega_n_ie = omega_ie * [ 0 sin(insLLHMinus(1)) 0; 21 | -sin(insLLHMinus(1)) 0 -cos(insLLHMinus(1)); 22 | 0 cos(insLLHMinus(1)) 0]; 23 | 24 | % define the transport rate term 25 | % this is due to the face that the local-navigation frame 26 | % center moves with respect to the Earth 27 | % Eq (5.42) 28 | 29 | % Radius of Curvature for North-South Motion (eq. 2.105) 30 | R_N = Ro*(1-ecc^2)/(1-ecc^2*sin(insLLHMinus(1))^2)^(3/2); 31 | % Radius of Curvature in East-West Direction (eq. 2.106) 32 | R_E = Ro/(1-ecc^2*sin(insLLHMinus(1))^2)^(1/2); 33 | 34 | % rotation rate vector 35 | omega_n_en = [ insVelMinus(2)/(R_E+insLLHMinus(3)); 36 | -insVelMinus(1)/(R_N+insLLHMinus(3)); 37 | (-insVelMinus(2)*tan(insLLHMinus(1)))/(R_E+insLLHMinus(3))]; 38 | % skew-symetric 39 | Omega_n_en = [0 -omega_n_en(3) omega_n_en(2); 40 | omega_n_en(3) 0 -omega_n_en(1); 41 | -omega_n_en(2) omega_n_en(1) 0]; 42 | 43 | % eq. 2.48 44 | omega_n_in=omega_n_en + omega_n_ie; 45 | 46 | alpha=omega_b_ib*dt; 47 | % lateralVelocity=norm([GeneratedEnuVelocity(:,1) GeneratedEnuVelocity(:,2)]); 48 | % integrate considering body-rate, Earth-rate, and craft-rate 49 | Cbb=eye(3)+(sin(norm(alpha))/(norm(alpha)))*(skewsymm(alpha)) + ((1-cos(norm(alpha)))/(norm(alpha))^2)*(skewsymm(alpha)).^2; 50 | 51 | Cb2nPlus= Cb2nMinus*(eye(3)+Omega_b_ib*dt)-(Omega_n_ie+Omega_n_en)*Cb2nMinus*dt; 52 | % Cb2nPlus= Cb2nMinus*(Cbb)-(Omega_n_ie+Omega_n_en)*Cb2nMinus*dt; 53 | % 54 | % omega_b_ie=CbnPlus'*omega_n_ie; 55 | % omega_b_ei=-omega_b_ie; 56 | % omega_b_eb=[Gx(i);Gy(i);Gz(i)]+omega_b_ei; 57 | 58 | insAttitudePlus=dcm2eulr(Cb2nPlus); %inputs Body to Nav 59 | end 60 | 61 | -------------------------------------------------------------------------------- /PostProcess_Matlab/LongRunData.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav/60662776a9a935303ab9b7de2d4616f9ef03fbd4/PostProcess_Matlab/LongRunData.mat -------------------------------------------------------------------------------- /PostProcess_Matlab/LongRunInit.m: -------------------------------------------------------------------------------- 1 | L=length(tTimuAdis); 2 | ba=zeros(3,L); 3 | bg=zeros(3,L); 4 | ba(:,1)=[7.549135895545244e-04;0.001805886718248;0.002395586011438]; %95 5 | % ba(:,1)=[-std(Ay(1:200));std(Ax(1:200));std(Az(1:200))]; 6 | % ba(:,1)=[-0.176671490035842;-0.0429240322667574;0.002395586011438]; 7 | bg(:,1)=[8.674066138832567e-05;1.005391303275586e-04;8.919962404601004e-05]; %95 8 | % bg(:,1)=[-std(Gy(1:200));std(Gx(1:200));std(Gz(1:200))]; 9 | % bg(:,1)=[-0.000190189015981112;0.000235157319569362;8.919962404601004e-05]; 10 | A=0.272; % meter 0.544/2 distance from IMU to Wheel 11 | % initHead=atan2(cos(lat1)*sin(lat2)-sin(lat1)*cos(lat2)*cos(lon2-lon1),sin(lon2-lon1)*cos(lat2)); 12 | T_r=0.125; % rear track width m 13 | s_or=0; 14 | sigma_or_L=.03;%0.03 15 | sigma_or_R=.03;%0.03 16 | sigma_cmc=.05; 17 | s_delta_or=0; 18 | 19 | x_err=zeros(15,1); 20 | transportRate=zeros(1,L); 21 | lateralVelocity=zeros(1,L); 22 | % P=blkdiag(((2*pi/180)^2)*(1/250),((2*pi/180)^2)*(1/250),(2*pi/180)^2,... % att 23 | % eye(3,3)*((0.01)^2)*(1/250),... % vel 24 | % eye(2,2)*(((10e-5)*pi/180)^2)*(1/250),1^2,... % pos 25 | % eye(3,3).*(ba(1:3)'.^2)*(1/250),... 26 | % eye(3,3).*(bg(1:3)'.^2)*(1/250)); 27 | 28 | P=blkdiag(((1*pi/180)^2)*(1/250),((1*pi/180)^2)*(1/250),((2*pi/180)^2)*(1/250),...%*(1/250),... % att 29 | eye(3,3)*((0.01)^2)*(1/250),... % vel 30 | eye(2,2)*(((10e-5)*pi/180)^2)*(1/250),0.1^2,... % pos 31 | eye(3).*ba(:,1),... 32 | eye(3).*bg(:,1)); 33 | sig1(1)=3*sqrt(P(1,1)); 34 | sig2(1)=3*sqrt(P(2,2)); 35 | sig3(1)=3*sqrt(P(3,3)); 36 | sig4(1)=3*sqrt(P(4,4)); 37 | sig5(1)=3*sqrt(P(5,5)); 38 | sig6(1)=3*sqrt(P(6,6)); 39 | sig7(1)=3*sqrt(P(7,7)); 40 | sig8(1)=3*sqrt(P(8,8)); 41 | sig9(1)=3*sqrt(P(9,9)); 42 | dt_odom(:,1)=0.1; 43 | x_State=zeros(15,L); 44 | x_State(:,1)=x_err(:,1); 45 | insAtt=zeros(3,L); 46 | insVel=zeros(3,L); 47 | insLLH=zeros(3,L); 48 | % insLLHCorr=zeros(3,L); 49 | omega_b_ib=zeros(1,3); 50 | f_ib_b=zeros(1,3); 51 | 52 | insAtt(:,1)=[0.0;0.0;85]*pi/180; 53 | insVel(:,1)=[0.0;0.0;0.0]'; 54 | insLLH(:,1)=[llhGPS(1,1),llhGPS(2,1),llhGPS(3,1)]; 55 | insLLHCorr(:,1)=insLLH(:,1); 56 | insXYZ(:,1)=[gpsECEF.x(1);gpsECEF.y(1);gpsECEF.z(1)]; 57 | xyzInit=insXYZ(:,1); 58 | 59 | TimeIMU=zeros(1,L); 60 | alpha_b_ib=zeros(1,3); 61 | v_ib_b=zeros(1,3); 62 | omega_ie = 7.292115e-5; % rotation of Earth in rad/sec 63 | Ro = 6378137; % WGS84 Equatorial Radius 64 | Rp = 6356752.31425; % WGS84 Polar Radius 65 | flat= 1/298.257223563; % WGS84 Earth flattening 66 | ecc = 0.0818191909425; % WGS84 Eccentricity 67 | TimeIMU(1)=0; 68 | TimeODOM(1)=0; 69 | H11=zeros(1,3); 70 | H12=zeros(1,3); 71 | H21=zeros(1,3); 72 | H31=zeros(1,3); 73 | H32=zeros(1,3); 74 | H41=zeros(1,3); 75 | H42=zeros(1,3); 76 | H24=zeros(1,3); 77 | z11=zeros(1,1); 78 | z21=zeros(1,1); 79 | z31=zeros(1,1); 80 | z41=zeros(1,1); 81 | kk=2; 82 | k=1; 83 | counter=2; 84 | bb(1)=1; 85 | dtTot=0; 86 | stationary(1)=0; 87 | statRate(1)=1; 88 | leverArm=zeros(3,1); 89 | cttr1=1; 90 | cttr2=1; 91 | cttr3=1; 92 | cttr0=1; 93 | offsetCtr=0; 94 | aaa=1; 95 | zeroUptCount=0; 96 | odomUptCount=1; 97 | 98 | x_gamma(:,1)=[0;0]; 99 | P_gamma=[0,0;0,0]; 100 | %% updates 101 | odomUpdate=true; 102 | zeroUpdate=true; 103 | nonHolo=true; 104 | backProp=false; 105 | gpsResults=true; 106 | contactAngle=true; 107 | -------------------------------------------------------------------------------- /PostProcess_Matlab/MAINRun.m: -------------------------------------------------------------------------------- 1 | %% Differential Wheel Rover Low Level Navigation post processing script 2 | % Authors: Cagri Kilic and Jason Gross 3 | % Date: September 2018 4 | %% ----------------------------------------------------------------------- 5 | % clc; 6 | clear; 7 | load LongRunData.mat % LshapeData.mat %ForwardDrive.mat 8 | %% odometry calculations 9 | odom 10 | %% initialize variables 11 | LongRunInit % LshapeInit.mat %ForwardDriveInit.mat 12 | 13 | %% Double Low Pass Filter 14 | dema 15 | for i=2:L 16 | dtIMU=tTimuAdis(i)-tTimuAdis(i-1); % calculates imu delta time from rostomat output tTimu 17 | TimeIMU(i)=dtIMU+TimeIMU(i-1); % creates imu time from delta time starting from zero 18 | %% ------------------------------------------------------------------- 19 | %% IMU mounting orientation for BadgerRover 20 | omega_b_ib=[-Gy(i),Gx(i),Gz(i)]-bg(:,i-1)'; %rad/s IMU gyro outputs minus estimated gyro bias 21 | f_ib_b = [-Ay(i),Ax(i),Az(i)]-ba(:,i-1)'; %m/s^2 IMU acceleration minus estimated acce bias 22 | v_ib_b= f_ib_b* dtIMU; %m/s acceleration times delta IMU = velocity 23 | %% -------------------------------------------------------------------- 24 | %% Attitude Update 25 | [insAttPlus, Cb2nPlus,Cb2nMinus,Omega_n_en,Omega_n_ie,R_N,R_E,omega_n_in,omega_n_ie] = AttUpdate(insAtt(:,i-1),omega_ie,insLLH(:,i-1),omega_b_ib,ecc,Ro,insVel(:,i-1),dtIMU); 26 | insAtt(:,i)=insAttPlus; 27 | %% Velocity Update 28 | [insVelPlus] = VelUpdate(Cb2nMinus, Cb2nPlus, v_ib_b,insVel(:,i-1),insLLH(:,i-1),omega_ie,Ro,ecc,dtIMU); 29 | insVel(:,i)=insVelPlus; 30 | %% Position Update 31 | [insLLHPlus,R_EPlus,r_eb_e] = PosUpdate(insLLH(:,i-1),insVel(:,i),insVel(:,i-1),Ro,ecc,dtIMU); 32 | insLLH(:,i)=insLLHPlus; 33 | insXYZ(:,i)=r_eb_e; 34 | %% -------------------------------------------------------------------- 35 | %% Error State Model--eq 14.63 36 | [STM] = insErrorStateModel_LNF(R_EPlus,R_N,insLLH(:,i),insVel(:,i),dtIMU,Cb2nPlus,omega_ie,omega_n_in,f_ib_b); 37 | %% Propagation of the Error 38 | x_err=STM*x_err; 39 | %% Q matrix -- 40 | F21= -skewsymm(Cb2nPlus*(f_ib_b')); 41 | Q=getQins(F21,Cb2nPlus,insLLH(:,i),R_N,R_E,dtIMU); 42 | [~,q] = chol(Q); 43 | if q ~= 0 44 | disp('Q matrix is not positive definite') 45 | i 46 | end 47 | %% P matrix 48 | P = STM*P*STM' + Q; 49 | [~,p] = chol(P); 50 | if p ~= 0 51 | disp('P matrix is not positive definite') 52 | i 53 | end 54 | if odomUpdate() 55 | %% -------------------------------------------------------------------- 56 | Cn2bPlus=Cb2nPlus'; 57 | omega_b_ie=Cn2bPlus*omega_n_ie; 58 | omega_b_ei=-omega_b_ie; 59 | omega_b_eb=omega_b_ei+omega_b_ib'; 60 | %% Measurement Matrix -- integration part for INS -- eq 16.48 61 | H11= H11+ [1,0,0]*Cn2bPlus*skewsymm(insVel(:,i))*dtIMU; 62 | H12= H12+ [1,0,0]*Cn2bPlus*dtIMU; 63 | H21= H21+ sin(insAtt(2,i))*[0,cos(insAtt(1,i)),sin(insAtt(1,i))]*Cn2bPlus*dtIMU; 64 | H31= H31+ [0,1,0]*Cn2bPlus*skewsymm(insVel(:,i))*dtIMU; 65 | H32= H32+ [0,1,0]*Cn2bPlus*dtIMU; 66 | H41= H41+ [0,0,1]*Cn2bPlus*skewsymm(insVel(:,i))*dtIMU; 67 | H42= H42+ [0,0,1]*Cn2bPlus*dtIMU; 68 | %% Measurement Innovation -- integration part for INS -- eq 16.42 69 | z11=z11+[1,0,0]*(Cn2bPlus*insVel(:,i) + skewsymm(omega_b_eb)*[-A,0,0]')*dtIMU; 70 | z21=z21+cos(insAtt(2,i))*dtIMU; 71 | z31=z31+[0,1,0]*(Cn2bPlus*insVel(:,i) + skewsymm(omega_b_eb)*[-A,0,0]')*dtIMU; 72 | z41=z41+[0,0,1]*(Cn2bPlus*insVel(:,i) + skewsymm(omega_b_eb)*[-A,0,0]')*dtIMU; 73 | % z and H values are integrated for IMU only above. When the Odometry 74 | % update is available (tTimu(i)>=tTodom(kk)) integrated values will be 75 | % updated by odometry measurements, used in the filter and destroyed. 76 | end 77 | if nonHolo() 78 | [insAtt(:,i),insVel(:,i),insLLH(:,i),x_err,P]= nonHolonomic(insVel(:,i),insAtt(:,i),insLLH(:,i),x_err,P,omega_n_ie,omega_b_ib,A); 79 | end 80 | %% -------------------------------------------------------------------- 81 | xState{1,i}=x_err; 82 | PStore{1,i}=P; 83 | STMStore{1,i}=STM+eye(15).*x_err; 84 | if kk=tTodom(kk) % Odometry update is available 88 | bb(counter)=i; % counts for the number of `i` when the loop goes into odometry updates 89 | dt_odom=tTodom(kk)-tTodom(kk-1); 90 | %% ------------------------------------------------------------ 91 | %% Odometry Update 92 | if odomUpdate() 93 | odomUptCount=odomUptCount+1; 94 | 95 | P_old=P; 96 | insAtt_old= insAtt(:,i); 97 | insVel_old= insVel(:,i); 98 | insLLH_old= insLLH(:,i); 99 | x_err_old=x_err; 100 | [insAtt(:,i),insVel(:,i),insLLH(:,i),x_err,P,postFitOdom]= odomUpt(insVel(:,i),insAtt(:,i),insLLH(:,i),x_err,P,... 101 | insAtt(:,i-1),insAtt(3,bb(counter-1)),dt_odom,rearVel(kk),headRate(kk),s_or,... 102 | H11,H12,H21,H31,H32,H41,H42,z11,z21,z31,z41,T_r,... 103 | sigma_or_L,sigma_or_R,sigma_cmc,s_delta_or); 104 | odompostfit(:,odomUptCount)=postFitOdom.z-postFitOdom.H*x_err; 105 | S=postFitOdom.H*P*postFitOdom.H'+[0.00045,0,0,0;0,0.1152,0,0;0,0,0.0025,0;0,0,0,0.0025]; 106 | chisq(:,odomUptCount)=odompostfit(:,odomUptCount)'*inv(S)*odompostfit(:,odomUptCount); 107 | mahala(:,odomUptCount)=sqrt(odompostfit(:,odomUptCount)'*inv(S)*odompostfit(:,odomUptCount)); 108 | 109 | if mahala(1,odomUptCount)>5 110 | 111 | % insAtt(:,i)=insAtt_old; 112 | % insVel(:,i)=insVel_old; 113 | % insLLH(:,i)=insLLH_old; 114 | % P=P_old; 115 | % x_err=x_err_old; 116 | LLHcorrected2(:,cttr2)=insLLH(:,i); 117 | cttr2=cttr2+1; 118 | end 119 | slipR(:,odomUptCount)=(rearVel(odomUptCount)-sqrt(insVel(1,i)^2+insVel(2,i)^2))/rearVel(odomUptCount); 120 | if slipR(:,odomUptCount) < -50 121 | slipR (:,odomUptCount) = 0; 122 | end 123 | if abs(slipR(1,odomUptCount))>0.25 124 | LLHcorrected1(:,cttr0)=insLLH(:,i); 125 | cttr0=cttr0+1; 126 | end 127 | end % odom update 128 | % % % ------------------------------------------------------------ 129 | % destroy H and z values for the next values 130 | H11=zeros(1,3); 131 | H12=zeros(1,3); 132 | H21=zeros(1,3); 133 | H31=zeros(1,3); 134 | H32=zeros(1,3); 135 | H24=zeros(1,3); 136 | H41=zeros(1,3); 137 | H42=zeros(1,3); 138 | z11=0; 139 | z21=0; 140 | z31=0; 141 | z41=0; 142 | counter=counter+1; 143 | kk=kk+1; 144 | end % odom update not available 145 | %% ---------------------------------------------------------------- 146 | 147 | if abs(lin_x(kk))<0.04% triggers zupt 148 | zeroUptCount=zeroUptCount+1; 149 | if zeroUpdate() 150 | [insVel(:,i),insAtt(:,i),insLLH(:,i),x_err,P,postFitZero] = zeroUpd(insVel(:,i),insAtt(:,i),insLLH(:,i),x_err,P,omega_b_ib); 151 | end 152 | zCtr(i)=cttr3+offsetCtr; 153 | LLHcorrected(:,cttr3)=insLLH(:,i); 154 | cttr3=cttr3+1; 155 | else 156 | zCtr(i)=0; 157 | offsetCtr=offsetCtr+1; 158 | end % zero update not available 159 | 160 | if backProp() 161 | if zCtr(i)-zCtr(i-1) < 0 % checks the zero update counter diff. If it is > 0 ZeroUpdate applied 162 | doBackProp=false; 163 | for j=i-1:-1:2 164 | if zCtr(j)-zCtr(j-1)<0 165 | doBackProp=true; 166 | lastZindex=j; 167 | break; 168 | end 169 | end 170 | if doBackProp 171 | x_err_s=xState{1,i}; 172 | P_s=PStore{1,i}; 173 | STM_s=STMStore{1,i}; 174 | for dd=i:-1:lastZindex 175 | [P_s,x_err_s] = smoothback(PStore{dd-1},PStore{dd},STMStore{dd-1},xState{dd-1},xState{dd},x_err_s,P_s,STM_s); 176 | insVel(:,dd)=insVel(:,dd)-x_err_s(4:6); 177 | insLLH(:,dd)=insLLH(:,dd)-x_err_s(7:9); 178 | Cn2b_propBack= eulr2dcm(insAtt(:,dd)); 179 | insAtt(:,dd)=dcm2eulr((eye(3)-skewsymm(x_err_s(1:3)))*Cn2b_propBack'); 180 | ba(1:3,dd)=x_err_s(10:12); 181 | bg(1:3,dd)=x_err_s(10:12); 182 | end 183 | else 184 | ba(1:3,i)=x_err(10:12); % acce bias, this value will be removed from IMU acce output 185 | bg(1:3,i)=x_err(13:15); % gyro bias, this value will be removed from IMU gyro output 186 | end % doBackProp 187 | end % if ZeroUpdate applied 188 | else 189 | % ba(1:3,i)=x_err(10:12); % acce bias, this value will be removed from IMU acce output 190 | % bg(1:3,i)=x_err(13:15); 191 | end 192 | end 193 | 194 | sig1(i)=3*sqrt(abs(P(1,1))); % 3 sigma values of att_x -roll 195 | sig2(i)=3*sqrt(abs(P(2,2))); % 3 sigma values of att_y -pitch 196 | sig3(i)=3*sqrt(abs(P(3,3))); % 3 sigma values of att_z -yaw 197 | 198 | sig4(i)=3*sqrt(abs(P(4,4))); % 3 sigma values of vel_x -forward 199 | sig5(i)=3*sqrt(abs(P(5,5))); % 3 sigma values of vel_y -left 200 | sig6(i)=3*sqrt(abs(P(6,6))); % 3 sigma values of vel_z -down 201 | 202 | sig7(i)=3*sqrt(abs(P(7,7))); % 3 sigma values of pos_x -latitude 203 | sig8(i)=3*sqrt(abs(P(8,8))); % 3 sigma values of pos_y -longitude 204 | sig9(i)=3*sqrt(abs(P(9,9))); % 3 sigma values of pos_z -height 205 | if gpsResults() 206 | gpsLonger(:,i)=[llhGPS(1,kk);llhGPS(2,kk);llhGPS(end,kk)]; 207 | end 208 | x_State(:,i)=[insAtt(:,i);insVel(:,i);insLLH(:,i);ba(:,i);bg(:,i)]; 209 | Cn2b_corr= eulr2dcm(insAtt(:,i)); 210 | insAttCorr(:,i)=dcm2eulr((eye(3)-skewsymm(x_err(1:3)))*Cn2b_corr'); 211 | insVelCorr(:,i)=insVel(:,i)-x_err(4:6); 212 | insLLHCorr(:,i)=insLLH(:,i)-x_err(7:9); 213 | 214 | 215 | if contactAngle() 216 | pitch=insAtt(2,i)*180/pi; 217 | pitch_rate=((insAtt(2,i)-insAtt(2,i-1))*180/pi)/dtIMU; 218 | wheelCenterSpeedFront=frontRightVel(kk)*0.1651; 219 | wheelCenterSpeedBack=rearLeftVel(kk)*0.1651; 220 | 221 | % if abs(pitch_rate)<1 && wheelCenterSpeedFront==0 && wheelCenterSpeedBack==0 % (pg:41) 222 | if wheelCenterSpeedFront==0 && wheelCenterSpeedBack==0 % (pg:41) 223 | 224 | % disp('Robot is stationary, infinite set of angles') 225 | x_gamma(:,i)=x_gamma(:,i-1); 226 | else 227 | x_gamma(:,i)=x_gamma(:,i-1); 228 | R_gamma=diag([0.001^2, 0.001^2]); 229 | P_gamma=P_gamma+R_gamma; 230 | [x_gamma(:,i),P_gamma]=wheelTerrainContactAngle(pitch,pitch_rate,wheelCenterSpeedFront,wheelCenterSpeedBack,x_gamma(:,i-1),P_gamma); 231 | psig1(i)=P_gamma(1,1); 232 | psig2(i)=P_gamma(2,2); 233 | if cos(x_gamma(2,i)-pitch)==0 234 | if wheelCenterSpeedFront==-wheelCenterSpeedBack 235 | disp('Pure Rotation Occurred') 236 | x_gamma(1,i)=pitch+pi/2 * sign(pitch_rate); 237 | x_gamma(2,i)=pitch-pi/2 * sign(pitch_rate); 238 | elseif pitch_rate==0 && wheelCenterSpeedFront==wheelCenterSpeedBack 239 | disp('Pure Translation Occurred, using previous solution') 240 | x_gamma(:,i)=x_gamma(:,i-1); 241 | end 242 | end 243 | end 244 | % end 245 | psig1(i)=P_gamma(1,1); 246 | psig2(i)=P_gamma(2,2); 247 | end 248 | 249 | end 250 | figureGeneration 251 | -------------------------------------------------------------------------------- /PostProcess_Matlab/PosUpdate.m: -------------------------------------------------------------------------------- 1 | function [insLLHPlus,R_EPlus,r_eb_e] = PosUpdate(insLLHMinus,insVelocityPlus,insVelocityMinus,Ro,ecc,dt) 2 | R_N = Ro*(1-ecc^2)/(1-ecc^2*sin(insLLHMinus(1))^2)^(3/2); 3 | % Radius of Curvature in East-West Direction (eq. 2.106) 4 | R_E = Ro/(1-ecc^2*sin(insLLHMinus(1))^2)^(1/2); 5 | 6 | % height update 7 | insLLHPlus(3,:)=insLLHMinus(3) - dt/2*(insVelocityMinus(3)+insVelocityPlus(3)); 8 | 9 | % latitude update 10 | insLLHPlus(1,:)=insLLHMinus(1)+ dt/2*(insVelocityMinus(1)/(R_N+insLLHMinus(3))+ insVelocityPlus(1)/(R_N+insLLHPlus(3))); 11 | 12 | % longitude update 13 | R_EPlus= Ro/(1-ecc^2*sin(insLLHPlus(1))^2)^(1/2); 14 | 15 | insLLHPlus(2,:)=insLLHMinus(2)+ dt/2*(insVelocityMinus(2)/((R_E+insLLHMinus(3))*cos(insLLHMinus(1))) ... 16 | + insVelocityPlus(2)/((R_EPlus+insLLHPlus(3))*cos(insLLHPlus(1)))); 17 | 18 | cos_lat = cos(insLLHPlus(1)); 19 | sin_lat = sin(insLLHPlus(1)); 20 | cos_long = cos(insLLHPlus(2)); 21 | sin_long = sin(insLLHPlus(2)); 22 | r_eb_e = [((R_E + insLLHPlus(3)) * cos_lat * cos_long);... 23 | ((R_E + insLLHPlus(3)) * cos_lat * sin_long);... 24 | (((1 - ecc^2) * R_E + insLLHPlus(3)) * sin_lat)]; 25 | 26 | end -------------------------------------------------------------------------------- /PostProcess_Matlab/Readme.md: -------------------------------------------------------------------------------- 1 | #Matlab files for post-processing 2 | Run the MAINRun.m 3 | -------------------------------------------------------------------------------- /PostProcess_Matlab/VelUpdate.m: -------------------------------------------------------------------------------- 1 | function [insVelPlus] = VelUpdate(Cb2nMinus, Cb2nPlus, v_ib_b,insVelMinus,insLLHMinus,omega_ie,Ro,ecc,dt) 2 | % specific-force transformation (eq. 5.48) 3 | V_n_ib=1/2*(Cb2nMinus+Cb2nPlus)*v_ib_b'; 4 | % Radius of Curvature for North-South Motion (eq. 2.105) 5 | R_N = Ro*(1-ecc^2)/(1-ecc^2*sin(insLLHMinus(1))^2)^(3/2); 6 | % Radius of Curvature in East-West Direction (eq. 2.106) 7 | R_E = Ro/(1-ecc^2*sin(insLLHMinus(1))^2)^(1/2); 8 | Omega_n_ie = omega_ie * [ 0 sin(insLLHMinus(1)) 0; 9 | -sin(insLLHMinus(1)) 0 -cos(insLLHMinus(1)); 10 | 0 cos(insLLHMinus(1)) 0]; 11 | % rotation rate vector 12 | omega_n_en = [ insVelMinus(2)/(R_E+insLLHMinus(3)); 13 | -insVelMinus(1)/(R_N+insLLHMinus(3)); 14 | (-insVelMinus(2)*tan(insLLHMinus(1)))/(R_E+insLLHMinus(3))]; 15 | % skew-symetric 16 | Omega_n_en = [0 -omega_n_en(3) omega_n_en(2); 17 | omega_n_en(3) 0 -omega_n_en(1); 18 | -omega_n_en(2) omega_n_en(1) 0]; 19 | % velocity integration considering, body to indertial velocity, 20 | % Earth-rate, Craft-rate, Gravity 21 | insVelPlus = insVelMinus + V_n_ib + ( [0;0;gravity(insLLHMinus(1),insLLHMinus(3))] - (Omega_n_en + 2*Omega_n_ie)*insVelMinus)*dt; 22 | end -------------------------------------------------------------------------------- /PostProcess_Matlab/backwardSmoothing.m: -------------------------------------------------------------------------------- 1 | doBackProp=false; 2 | for j=i-1:-1:2 3 | if zCtr(j)-zCtr(j-1)<0 4 | doBackProp=true; 5 | lastZindex=j; 6 | break; 7 | end 8 | end 9 | if doBackProp 10 | x_err_s=xState{1,i}; 11 | P_s=PStore{1,i}; 12 | STM_s=STMStore{1,i}; 13 | for dd=i:-1:lastZindex 14 | [P_s,x_err_s] = smoothback(PStore{dd-1},PStore{dd},STMStore{dd-1},xState{dd-1},xState{dd},x_err_s,P_s,STM_s); 15 | insVel(:,dd)=insVel(:,dd)-x_err_s(4:6); 16 | insLLH(:,dd)=insLLH(:,dd)-x_err_s(7:9); 17 | Cn2b_propBack= eulr2dcm(insAtt(:,dd)); 18 | insAtt(:,dd)=dcm2eulr((eye(3)-skewsymm(x_err_s(1:3)))*Cn2b_propBack'); 19 | ba(1:3,dd)=x_err_s(10:12); 20 | bg(1:3,dd)=x_err_s(10:12); 21 | end 22 | else 23 | ba(1:3,i)=x_err(10:12); % acce bias, this value will be removed from IMU acce output 24 | bg(1:3,i)=x_err(13:15); % gyro bias, this value will be removed from IMU gyro output 25 | end % doBackProp -------------------------------------------------------------------------------- /PostProcess_Matlab/circle.m: -------------------------------------------------------------------------------- 1 | function h = circle(x,y,r) 2 | hold on 3 | th = 0:pi/5:2*pi; 4 | xunit = r * cos(th) + x; 5 | yunit = r * sin(th) + y; 6 | h = plot(xunit, yunit); 7 | hold off -------------------------------------------------------------------------------- /PostProcess_Matlab/dcm2eulr.m: -------------------------------------------------------------------------------- 1 | function eul_vect = dcm2eulr(DCMbn) 2 | %DCM2EULR Direction cosine matrix to Euler angle 3 | 4 | % vector conversion. 5 | % 6 | % eul_vect = dcm2eulr(DCMbn) 7 | % 8 | % INPUTS 9 | % DCMbn = 3x3 direction cosine matrix providing the 10 | % transformation from the body frame 11 | % to the navigation frame 12 | % 13 | % OUTPUTS 14 | % eul_vect(1) = roll angle in radians 15 | % 16 | % eul_vect(2) = pitch angle in radians 17 | % 18 | % eul_vect(3) = yaw angle in radians 19 | % 20 | % NOTE 21 | % If the pitch angle is vanishingly close to +/- pi/2, 22 | % the elements of EUL_VECT will be filled with NaN. 23 | 24 | % REFERENCE: Titterton, D. and J. Weston, STRAPDOWN 25 | % INERTIAL NAVIGATION TECHNOLOGY, Peter 26 | % Peregrinus Ltd. on behalf of the Institution 27 | % of Electrical Engineers, London, 1997. 28 | % 29 | % M. & S. Braasch 12-97; 02-05; 04-06 30 | % Copyright (c) 1997-2006 by GPSoft LLC 31 | % All Rights Reserved. 32 | % 33 | 34 | if nargin<1,error('insufficient number of inpust arguments'),end 35 | 36 | phi = atan2(DCMbn(3,2),DCMbn(3,3)); 37 | theta = asin(-DCMbn(3,1)); 38 | psi = atan2(DCMbn(2,1),DCMbn(1,1)); 39 | 40 | eul_vect = [phi theta psi]'; 41 | 42 | % eul_vect = [phi theta psi]'; 43 | % %Grove's 44 | % eul(1,1) = atan2(C(2,3),C(3,3)); % roll 45 | % eul(2,1) = - asin(C(1,3)); % pitch 46 | % eul(3,1) = atan2(C(1,2),C(1,1)); % yaw 47 | 48 | -------------------------------------------------------------------------------- /PostProcess_Matlab/dema.m: -------------------------------------------------------------------------------- 1 | 2 | imu_filtAX(:,1) = Ax(1); 3 | imu_filtAY(:,1) = Ay(1); 4 | imu_filtAZ(:,1) = Az(1); 5 | imu_filtGX(:,1) = Gx(1); 6 | imu_filtGY(:,1) = Gy(1); 7 | imu_filtGZ(:,1) = Gz(1); 8 | 9 | imu_filt2AX(:,1) = 0; 10 | imu_filt2AY(:,1) = 0; 11 | imu_filt2AZ(:,1) = Az(1); 12 | imu_filt2GX(:,1) = 0; 13 | imu_filt2GY(:,1) = 0; 14 | imu_filt2GZ(:,1) = 0; 15 | alpha_=0.0475; 16 | 17 | for i=2:length(Ax) 18 | imu_rawAX(:,i)=Ax(i); 19 | imu_rawAY(:,i)=Ay(i); 20 | imu_rawAZ(:,i)=Az(i); 21 | imu_rawGX(:,i)=Gx(i); 22 | imu_rawGY(:,i)=Gy(i); 23 | imu_rawGZ(:,i)=Gz(i); 24 | 25 | imu_filtAX(:,i) = alpha_ * imu_rawAX(:,i) + (1-alpha_) * imu_filtAX(:,i-1); 26 | imu_filtAY(:,i) = alpha_ * imu_rawAY(:,i) + (1-alpha_) * imu_filtAY(:,i-1); 27 | imu_filtAZ(:,i) = alpha_ * imu_rawAZ(:,i) + (1-alpha_) * imu_filtAZ(:,i-1); 28 | imu_filtGX(:,i) = alpha_ * imu_rawGX(:,i) + (1-alpha_) * imu_filtGX(:,i-1); 29 | imu_filtGY(:,i) = alpha_ * imu_rawGY(:,i) + (1-alpha_) * imu_filtGY(:,i-1); 30 | imu_filtGZ(:,i) = alpha_ * imu_rawGZ(:,i) + (1-alpha_) * imu_filtGZ(:,i-1); 31 | 32 | imu_filt2AX(:,i) = alpha_ * imu_filtAX(:,i) + (1-alpha_) * imu_filt2AX(:,i-1); 33 | imu_filt2AY(:,i) = alpha_ * imu_filtAY(:,i) + (1-alpha_) * imu_filt2AY(:,i-1); 34 | imu_filt2AZ(:,i) = alpha_ * imu_filtAZ(:,i) + (1-alpha_) * imu_filt2AZ(:,i-1); 35 | imu_filt2GX(:,i) = alpha_ * imu_filtGX(:,i) + (1-alpha_) * imu_filt2GX(:,i-1); 36 | imu_filt2GY(:,i) = alpha_ * imu_filtGY(:,i) + (1-alpha_) * imu_filt2GY(:,i-1); 37 | imu_filt2GZ(:,i) = alpha_ * imu_filtGZ(:,i) + (1-alpha_) * imu_filt2GZ(:,i-1); 38 | 39 | imu_filt3AX(:,i) = 2* imu_filtAX(:,i) - imu_filt2AX(:,i); 40 | imu_filt3AY(:,i) = 2* imu_filtAY(:,i) - imu_filt2AY(:,i); 41 | imu_filt3AZ(:,i) = 2* imu_filtAZ(:,i) - imu_filt2AZ(:,i); 42 | imu_filt3GX(:,i) = 2* imu_filtGX(:,i) - imu_filt2GX(:,i); 43 | imu_filt3GY(:,i) = 2* imu_filtGY(:,i) - imu_filt2GY(:,i); 44 | imu_filt3GZ(:,i) = 2* imu_filtGZ(:,i) - imu_filt2GZ(:,i); 45 | 46 | end 47 | Ax=imu_filt3AX; 48 | Ay=imu_filt3AY; 49 | Az=imu_filt3AZ; 50 | 51 | Gx=imu_filt3GX; 52 | Gy=imu_filt3GY; 53 | Gz=imu_filt3GZ; 54 | -------------------------------------------------------------------------------- /PostProcess_Matlab/ellipse3D.m: -------------------------------------------------------------------------------- 1 | function [ellipseCoords, h] = ellipse3D( rx,ry,x0,y0,z0,Nb,pitch,roll,yaw,graph,C ) 2 | %% Definition 3 | % 4 | % Ellipse3D generates a three row, single column vector that holds the 5 | % coordinates of an ellipse in 3D space and can optionally plot it. 6 | % 7 | % Generation of the ellipse coordinates occurs via the following 8 | % methodology: 9 | % 10 | % 1. Plot an ellipse on the XY plane with semimajor axis of radius rx 11 | % along X axis and semimajor axis of radius ry along Y axis 12 | % 13 | % 2. Rotate ellipse CCW about X axis by 'pitch' radians. 0 leaves ellipse 14 | % on XY plane. pi/2 rotates CCW about positive X-axis and puts ellipse 15 | % on XZ plane 16 | % 17 | % 3. Rotate ellipse CCW about Y axis by 'roll' radians. 0 leaves 18 | % ellipse on XY plane. pi/2 rotates CCW about positive Y-axis and puts 19 | % ellipse on YZ plane 20 | % 21 | % 4. Rotate ellipse CCW about Z axis by 'yaw' 22 | % radians. 0 leaves ellipse on XY plane. pi/2 rotates CCW about 23 | % positive Z-axis and (leaving ellipse on XY plane) 24 | % 25 | % 5. Apply offsets of x0, y0, and z0 to ellipse 26 | % 27 | % 6. Optionally generate 3D plot of ellipse 28 | % 29 | %% Inputs 30 | % 31 | % rx - length of radius of semimajor axis of ellipse along x-axis 32 | % ry - length of radius of semimajor axis of ellipse along y-axis 33 | % x0 - origin of ellipse with respect to x-axis 34 | % y0 - origin of ellipse with respect to y-axis 35 | % z0 - origin of ellipse with respect to z-axis 36 | % Nb - number of points used to define ellipse 37 | % pitch - angle of pitch in radians of ellipse wRt +x-axis 38 | % roll - angle of roll in radians of ellipse wRt +y-axis 39 | % yaw - angle of yaw in radians of ellipse wRt +z-axis 40 | % graph - flag that tells function whether or not to graph. 41 | % 0 - do not graph 42 | % 1 - graph and let this function define graphing options 43 | % >1 - graph, but allow other functions to define graphing options 44 | % C - color of ellipse to be plotted. Acceptable input either in character 45 | % form ('r') or RGB form ([0 .5 1]) 46 | % 47 | %% Outputs 48 | % 49 | % ellipseCoords - holds coordinates of ellipse in form: 50 | % [ X 51 | % Y 52 | % Z ] 53 | % h - graphics handle of ellipse if graphed, 0 otherwise 54 | %% Usage Examples 55 | % ELLIPSE(rx,ry,x0,y0,z0) adds an on the XY plane ellipse with semimajor 56 | % axis of rx, a semimajor axis of radius ry centered at the point x0,y0,z0. 57 | % 58 | % ELLIPSE(rx,ry,x0,y0,z0,Nb), Nb specifies the number of points 59 | % used to draw the ellipse. The default value is 300. Nb may be used 60 | % for each ellipse individually. 61 | % 62 | % ELLIPSE(rx,ry,x0,y0,z0,Nb, pitch,roll,yaw) adds an on the XY plane 63 | % ellipse with semimajor axis of rx, a semimajor axis of radius ry centered 64 | % at the point x0,y0,z0 and a pose in 3D space defined by roll, pitch, and 65 | % yaw angles 66 | % 67 | % as a sample of how ellipse works, the following produces a red ellipse 68 | % tipped up with a pitch of 45 degrees 69 | % [coords, h] = ellipse3D(1,2,0,0,0,300,pi/4,0,0,1,'r'); 70 | % 71 | % note that if rx=ry, ELLIPSE3D plots a circle 72 | % 73 | %% METADATA 74 | % Author: William Martin 75 | % Author Contact: wmartin8@utk.edu 76 | % Institution: University of Tennessee, Knoxville 77 | % Date Created: 6/22/2012 78 | % Last Updated: 6/25/2012 79 | % Inspiration: 80 | % ellipse.m 81 | % written by D.G. Long, Brigham Young University, based on the 82 | % CIRCLES.m original 83 | % written by Peter Blattner, Institute of Microtechnology, University of 84 | % Neuchatel, Switzerland, blattner@imt.unine.ch 85 | %% Parse Inputs 86 | % Check the number of input arguments 87 | if nargin<=1,error('Not enough arguments');end; 88 | if nargin<1,rx=[];end; 89 | if nargin<2,ry=[];end; 90 | if nargin<3,x0=[];end; 91 | if nargin<5,x0=[];y0=[];z0=[];end; 92 | if nargin<6,Nb=[];end 93 | if nargin<7,pitch=[];end 94 | if nargin<8,roll=[];end 95 | if nargin<9,yaw=[];end 96 | if nargin<10,graph=[];end 97 | if nargin<11,C=[];end 98 | % set up the default values 99 | if isempty(rx),rx=1;end; 100 | if isempty(ry),ry=1;end; 101 | if isempty(x0),x0=0;end; 102 | if isempty(y0),y0=0;end; 103 | if isempty(z0),z0=0;end; 104 | if isempty(Nb),Nb=300;end; 105 | if isempty(pitch),pitch=0;end; 106 | if isempty(roll),roll=0;end; 107 | if isempty(yaw),yaw=0;end; 108 | if isempty(graph),graph=1;end; 109 | if isempty(C),C='b';end; 110 | h=0; 111 | if ischar(C) 112 | C=C(:); 113 | else 114 | C=[0,0,1]; % Make blue 115 | end 116 | % Ensure that all input values are scalars 117 | if length(rx)>1,error('too many values for rx');end; 118 | if length(ry)>1,error('too many values for ry');end; 119 | if length(x0)>1,error('too many values for x0');end; 120 | if length(y0)>1,error('too many values for y0');end; 121 | if length(z0)>1,error('too many values for z0');end; 122 | if length(Nb)>1,error('too many values for Nb');end; 123 | if length(pitch)>1,error('too many values for pitch');end; 124 | if length(roll)>1,error('too many values for roll');end; 125 | if length(yaw)>1,error('too many values for yaw');end; 126 | if length(graph)>1,error('too many values for graph');end; 127 | %% Mathematical Formulation 128 | % Declare angle vector theta (t in parametric equation of ellipse) 129 | the=linspace(0,2*pi,Nb); 130 | % Create X and Y vectors using parametric equation of ellipse 131 | X=rx*cos(the); 132 | Y=ry*sin(the); 133 | % Declare the Z plane as all zeros before transformation 134 | Z=zeros(1, length(X)); 135 | % Define rotation matrix about X axis. 0 leaves ellipse on XY plane. pi/2 136 | % rotates CCW about X-axis and puts ellipse on XZ plane 137 | Rpitch = [1 0 0 ;... 138 | 0 cos(pitch) -sin(pitch);... 139 | 0 sin(pitch) cos(pitch)]; 140 | % Define rotation matrix about Y axis. 0 leaves ellipse on XY plane. pi/2 141 | % rotates CCW about Y-axis and puts ellipse on YZ plane 142 | Rroll = [cos(roll) 0 sin(roll) ;... 143 | 0 1 0 ;... 144 | -sin(roll) 0 cos(roll)] ; 145 | % Define rotation matrix about about Z axis. 0 leaves ellipse on XY plane. 146 | % pi/2 rotates CCW about Z-axis and (leaving ellipse on XY plane) 147 | Ryaw = [cos(yaw) -sin(yaw) 0;... 148 | sin(yaw) cos(yaw) 0;... 149 | 0 0 1]; 150 | % Apply transformation 151 | for i=1:length(X) 152 | 153 | xyzMat = [X(i);Y(i);Z(i)]; % create temp values 154 | temp = Rpitch*xyzMat; % apply pitch 155 | temp = Rroll*temp; % apply roll 156 | temp = Ryaw*temp; % apply yaw 157 | X(i) = temp(1); % store results 158 | Y(i) = temp(2); 159 | Z(i) = temp(3); 160 | 161 | end 162 | % Apply offsets 163 | X = X + x0; 164 | Y = Y + y0; 165 | Z = Z + z0; 166 | %% Graphing 167 | if graph > 0 168 | 169 | if graph == 0 170 | axis equal; 171 | axis auto; 172 | axis vis3d; 173 | grid on; 174 | view(3); 175 | 176 | axisLength = max([rx ry]) + max([abs(x0) abs(y0) abs(z0)]); 177 | % h3=line([0,axisLength],[0,0],[0,0]); 178 | % set(h3,'Color','b'); 179 | % text(axisLength,0,0,'X'); 180 | 181 | % h2=line([0,0],[0,axisLength],[0,0]); 182 | % set(h2,'Color','g'); 183 | % text(0,axisLength,0,'Y'); 184 | % 185 | % h1=line([0,0],[0,0],[0,axisLength]); 186 | % set(h1,'Color','r'); 187 | % text(0,0,axisLength,'Z'); 188 | 189 | h = line(X,Y,Z); % plot ellipse in 3D 190 | set(h,'color',C); % Set color of ellipse 191 | else 192 | 193 | h = line(X,Y,Z); % plot ellipse in 3D 194 | set(h,'color',C); % Set color of ellipse 195 | end 196 | 197 | end 198 | ellipseCoords = [X;Y;Z]; % define return values 199 | end 200 | -------------------------------------------------------------------------------- /PostProcess_Matlab/ellipsegen.m: -------------------------------------------------------------------------------- 1 | hold off 2 | 3 | loyolagray = 1/255*[100,100,100]; 4 | for ii=1:750:length(ENU(1:100000)) 5 | d(ii) = pdist([ENU(1,ii),ENU(2,ii);ENU3P(1,ii),ENU3P(2,ii)],'euclidean'); 6 | b(ii) = pdist([ENU(2,ii),ENU(3,ii);ENU3P(2,ii),ENU3P(3,ii)],'euclidean'); 7 | coords(:,:,ii)=ellipse3D(d(ii),b(ii),ENU(1,ii),ENU(2,ii),ENU(3,ii),300,pi/2,0,insAtt(3,ii)); 8 | % plot3([coords]') 9 | % ELLIPSE(rx,ry,x0,y0,z0,Nb, pitch,roll,yaw) adds an on the XY plane 10 | % ellipse with semimajor axis of rx, a semimajor axis of radius ry centered 11 | % at the point x0,y0,z0 and a pose in 3D space defined by roll, pitch, and 12 | % yaw angles 13 | % plot(x,y,'k')% xxx(kkk)=x; 14 | hold on 15 | plot3(coords(1,:,ii),coords(2,:,ii),coords(3,:,ii),'Color',loyolagray) 16 | 17 | end 18 | 19 | plot3(ENU(1,:),ENU(2,:),ENU(3,:),'.r') 20 | hold on 21 | plot3(ENU3P(1,:),ENU3P(2,:),ENU3P(3,:),'-k') 22 | hold on 23 | plot3(ENU_3P(1,:),ENU_3P(2,:),ENU_3P(3,:),'-k') 24 | hold on 25 | plot3(ENUGPS(1,:),ENUGPS(2,:),ENUGPS(3,:),'.-b') 26 | grid on 27 | view(-73,40) 28 | hold off 29 | -------------------------------------------------------------------------------- /PostProcess_Matlab/error_ellipse.m: -------------------------------------------------------------------------------- 1 | NP = 16; 2 | alpha = 2*pi/NP*(0:NP); 3 | circle = [cos(alpha);sin(alpha)]; 4 | ns = 3; 5 | x = [0.691952541453085;-1.39574109762468]; 6 | P = [1.28377282100450e-14,3.02902408861744e-17;3.02902380398945e-17,1.27094882859485e-14]; 7 | C = chol(P)'; 8 | ellip = ns*C*circle; 9 | X = x(1)+ellip(1,:); 10 | Y = x(2)+ellip(2,:); 11 | % [45.8918521781714;23.7020620139446;-2.32985829541740] -------------------------------------------------------------------------------- /PostProcess_Matlab/eulr2dcm.m: -------------------------------------------------------------------------------- 1 | function DCMnb = eulr2dcm(eul_vect) 2 | %EULR2DCM Euler angle vector to direction cosine 3 | % matrix conversion. 4 | % 5 | % DCMnb = eulr2dcm(eul_vect) 6 | % 7 | % INPUTS 8 | % eul_vect(1) = roll angle in radians 9 | % 10 | % eul_vect(2) = pitch angle in radians 11 | % 12 | % eul_vect(3) = yaw angle in radians 13 | % 14 | % OUTPUTS 15 | % DCMnb = 3x3 direction cosine matrix providing the 16 | % transformation from the navigation frame (NED) 17 | % to the body frame 18 | % 19 | 20 | % REFERENCE: Titterton, D. and J. Weston, STRAPDOWN 21 | % INERTIAL NAVIGATION TECHNOLOGY, Peter 22 | % Peregrinus Ltd. on behalf of the Institution 23 | % of Electrical Engineers, London, 1997. 24 | % 25 | % M. & S. Braasch 12-97; 02-05 26 | % Copyright (c) 1997-2005 by GPSoft LLC 27 | % All Rights Reserved. 28 | 29 | if nargin<1,error('insufficient number of input arguments'),end 30 | 31 | 32 | phi = eul_vect(1); theta = eul_vect(2); psi = eul_vect(3); 33 | 34 | cpsi = cos(psi); spsi = sin(psi); 35 | cthe = cos(theta); sthe = sin(theta); 36 | cphi = cos(phi); sphi = sin(phi); 37 | 38 | C1 = [cpsi spsi 0; ... 39 | -spsi cpsi 0; ... 40 | 0 0 1]; 41 | 42 | C2 = [cthe 0 -sthe; ... 43 | 0 1 0 ; ... 44 | sthe 0 cthe]; 45 | C3 = [1 0 0; ... 46 | 0 cphi sphi; ... 47 | 0 -sphi cphi]; 48 | DCMnb = C3*C2*C1; 49 | 50 | % sin_phi = sin(eul_vect(1)); 51 | % cos_phi = cos(eul_vect(1)); 52 | % sin_theta = sin(eul_vect(2)); 53 | % cos_theta = cos(eul_vect(2)); 54 | % sin_psi = sin(eul_vect(3)); 55 | % cos_psi = cos(eul_vect(3)); 56 | % 57 | % % Calculate coordinate transformation matrix using (2.22) 58 | % DCMnb(1,1) = cos_theta * cos_psi; 59 | % DCMnb(1,2) = cos_theta * sin_psi; 60 | % DCMnb(1,3) = -sin_theta; 61 | % DCMnb(2,1) = -cos_phi * sin_psi + sin_phi * sin_theta * cos_psi; 62 | % DCMnb(2,2) = cos_phi * cos_psi + sin_phi * sin_theta * sin_psi; 63 | % DCMnb(2,3) = sin_phi * cos_theta; 64 | % DCMnb(3,1) = sin_phi * sin_psi + cos_phi * sin_theta * cos_psi; 65 | % DCMnb(3,2) = -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi; 66 | % DCMnb(3,3) = cos_phi * cos_theta; -------------------------------------------------------------------------------- /PostProcess_Matlab/figGen.m: -------------------------------------------------------------------------------- 1 | h(1)=figure; 2 | subplot(311), 3 | hold on 4 | plot(TimeIMU,insAtt(1,:)*180/pi,'-.r','DisplayName','estimation') 5 | % hold on 6 | % plot(TimeIMU,sig1(1,:)*180/pi,'-.b','DisplayName','Cov') 7 | ylabel('\phi_E_s_t (deg)') 8 | legend('show'); 9 | 10 | PitchErr2=x_err(2,:)*180/pi;%*180/pi; 11 | subplot(312), 12 | hold on 13 | plot(TimeIMU,insAtt(2,:)*180/pi,'-.r','DisplayName','Model') 14 | ylabel('\theta_E_s_t (deg)') 15 | legend('show'); 16 | YawErr2=insAtt(3,:)*180/pi;%*180/pi; 17 | subplot(313), 18 | hold on 19 | plot(TimeIMU,YawErr2,'-.r','DisplayName','Model') 20 | ylabel('\psi_E_s_t (deg)') 21 | 22 | 23 | VerrN2= x_err(4,:)';%-GeneratedEnuVelocity(:,2); 24 | VerrE2= x_err(5,:)';%-GeneratedEnuVelocity(:,1); 25 | VerrD2= x_err(6,:)';%+GeneratedEnuVelocity(:,3); 26 | 27 | h(2)=figure; 28 | subplot(311), 29 | hold on 30 | plot(TimeIMU,insVel(1,:),'-.r','DisplayName','Model') 31 | ylabel('V_N E_s_t (m/s)') 32 | legend('show'); 33 | hold off 34 | subplot(312), 35 | hold on 36 | plot(TimeIMU,insVel(2,:),'-.r','DisplayName','Model') 37 | ylabel('V_E E_s_t (m/s)') 38 | hold off 39 | legend('show'); 40 | subplot(313), 41 | hold on 42 | plot(TimeIMU,insVel(3,:),'-.r','DisplayName','Model') 43 | ylabel('V_D E_s_t (m/s)') 44 | legend('show'); 45 | hold off 46 | xlabel('TimeIMU ') 47 | llhEst=zeros(3,L); 48 | llhTrue=zeros(3,L); 49 | llhEst2=zeros(3,L); 50 | 51 | % figure, 52 | % subplot(311), 53 | % hold on 54 | % plot(TimeIMU,insLLH(1,:)*180/pi,'-.r','DisplayName','Model') 55 | % ylabel('lat E_s_t ') 56 | % legend('show'); 57 | % hold off 58 | % subplot(312), 59 | % plot(TimeIMU,insLLH(2,:)*180/pi,'-.r','DisplayName','Model') 60 | % ylabel('lon E_s_t ') 61 | % legend('show'); 62 | % hold off 63 | % subplot(313), 64 | % hold on 65 | % plot(TimeIMU,insLLH(3,:),'-.r','DisplayName','Model') 66 | % ylabel('h E_s_t '); 67 | % legend('show'); 68 | % hold off 69 | % xlabel('TimeIMU ') 70 | 71 | 72 | h(3)=figure; 73 | fill( [tTimu(1:end-1,1)'-tTimu(1) fliplr(tTimu(1:end-1,1)'-tTimu(1))], [ENU_3P(1,:) fliplr(ENU3P(1,:))], 'r','DisplayName','3\sigma Covariance Hull' ); 74 | hold on 75 | plot(tTimu(1:end-1,1)-tTimu(1),ENU(1,:),'-b','DisplayName','Filter Estimation East'); 76 | if gpsResults() 77 | hold on; 78 | plot(tTodom-min(tTodom),ENUGPS(1,1:length(tTodom)),'-k','DisplayName','GPS Solution East'); 79 | end 80 | % hold on; 81 | % plot(tTodom-min(tTodom),Pos_x-Pos_x(1),'-.g','DisplayName','odom') 82 | % hold on 83 | 84 | % hold on 85 | % patch(ENU_3P(1,:), ENU3P(1,:), [.8 .8 .1]); 86 | ylabel('E (m)') 87 | xlabel('time (sec)') 88 | legend('show'); 89 | 90 | h(4)=figure; 91 | 92 | fill( [tTimu(1:end-1,1)'-tTimu(1) fliplr(tTimu(1:end-1,1)'-tTimu(1))], [ENU_3P(2,:) fliplr(ENU3P(2,:))], 'r','DisplayName','3\sigma Covariance Hull' ); 93 | hold on 94 | plot(tTimu(1:end-1,1)-tTimu(1),ENU(2,:),'-b','DisplayName','Filter Estimation North'); 95 | if gpsResults() 96 | hold on; 97 | plot(tTodom-min(tTodom),ENUGPS(2,1:length(tTodom)),'-k','DisplayName','GPS Solution North'); 98 | end 99 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU(2,:),'-.b','DisplayName','LLN'); 100 | % hold on; 101 | % plot(tTodom-min(tTodom),ENUGPS(2,1:length(tTodom)),'-.r','DisplayName','true') 102 | % hold on; 103 | % % plot(tTodom-min(tTodom),Pos_y-Pos_y(1),'-.g','DisplayName','odom') 104 | % hold on 105 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU3P(2,:),'--k') 106 | % hold on 107 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU_3P(2,:),'--k') 108 | ylabel('N (m)') 109 | xlabel('time (sec)') 110 | legend('show'); 111 | 112 | h(5)=figure; 113 | 114 | fill( [tTimu(1:end-1,1)'-tTimu(1) fliplr(tTimu(1:end-1,1)'-tTimu(1))], [ENU_3P(3,:) fliplr(ENU3P(3,:))], 'r','DisplayName','3\sigma Covariance Hull' ); 115 | hold on 116 | plot(tTimu(1:end-1,1)-tTimu(1),ENU(3,:),'-b','DisplayName','Filter Estimation Up'); 117 | if gpsResults() 118 | hold on; 119 | plot(tTodom-min(tTodom),ENUGPS(3,1:length(tTodom)),'-k','DisplayName','GPS Solution Up'); 120 | end 121 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU(3,:)-ENU(3,1),'-.b','DisplayName','LLN'); 122 | % hold on; 123 | % plot(tTodom-min(tTodom),ENUGPS(3,1:length(tTodom)),'-.r','DisplayName','true') 124 | % hold on 125 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU3P(3,:),'--k') 126 | % hold on 127 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU_3P(3,:),'--k') 128 | ylabel('U (m)') 129 | xlabel('time (sec)') 130 | legend('show'); 131 | 132 | % h(3)=figure; 133 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU(1,:),'-.b','DisplayName','LLN'); 134 | % hold on; 135 | % plot(tTodom-min(tTodom),ENUGPS(1,1:length(tTodom)),'-.r','DisplayName','true') 136 | % % hold on; 137 | % % plot(tTodom-min(tTodom),Pos_x-Pos_x(1),'-.g','DisplayName','odom') 138 | % hold on 139 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU3P(1,:),'--k') 140 | % hold on 141 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU_3P(1,:),'--k') 142 | % ylabel('E (m)') 143 | % xlabel('time (sec)') 144 | % legend('show'); 145 | % 146 | % 147 | % h(4)=figure; 148 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU(2,:),'-.b','DisplayName','LLN'); 149 | % hold on; 150 | % plot(tTodom-min(tTodom),ENUGPS(2,1:length(tTodom)),'-.r','DisplayName','true') 151 | % hold on; 152 | % % plot(tTodom-min(tTodom),Pos_y-Pos_y(1),'-.g','DisplayName','odom') 153 | % hold on 154 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU3P(2,:),'--k') 155 | % hold on 156 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU_3P(2,:),'--k') 157 | % ylabel('N (m)') 158 | % xlabel('time (sec)') 159 | % legend('show'); 160 | % 161 | % h(5)=figure; 162 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU(3,:)-ENU(3,1),'-.b','DisplayName','LLN'); 163 | % hold on; 164 | % plot(tTodom-min(tTodom),ENUGPS(3,1:length(tTodom)),'-.r','DisplayName','true') 165 | % hold on 166 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU3P(3,:),'--k') 167 | % hold on 168 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU_3P(3,:),'--k') 169 | % ylabel('U (m)') 170 | % xlabel('time (sec)') 171 | % legend('show'); 172 | 173 | % figure; 174 | % plot(tTimu-min(tTimu),insLLH(2,:)*180/pi,'-.b','DisplayName','LLN'); 175 | % hold on; 176 | % plot(tTodom-min(tTodom),llhGPS(2,2:end)*180/pi,'-.r','DisplayName','true') 177 | % ylabel('Longitude') 178 | % xlabel('time (sec)') 179 | % legend('show'); 180 | 181 | % figure; 182 | % plot(tTimu-min(tTimu),insLLH(1,:)*180/pi,'-.b','DisplayName','LLN'); 183 | % hold on; 184 | % plot(tTodom-min(tTodom),llhGPS(1,2:end)*180/pi,'-.r','DisplayName','true') 185 | % ylabel('Latitude ') 186 | % xlabel('time (sec)') 187 | % legend('show'); 188 | 189 | h(6)=figure; 190 | plot(ENU(1,:),ENU(2,:),'.--r','DisplayName','CoreNav') 191 | hold on 192 | plot(ENU(1,1),ENU(2,1),'o','DisplayName','CoreNav Start') 193 | hold on 194 | plot(ENU(1,end),ENU(2,end),'*','DisplayName','CoreNav End') 195 | if gpsResults() 196 | hold on 197 | plot(ENUGPS(1,:),ENUGPS(2,:),'.-k','DisplayName','GPS ENU') 198 | end 199 | hold on 200 | plot(ENUcorrected(1,:),ENUcorrected(2,:),'--og','DisplayName','CoreNav') 201 | 202 | if odomUpdate 203 | hold on 204 | plot(ENUcorrected1(1,:),ENUcorrected1(2,:),'*b','DisplayName','Slip') 205 | hold on 206 | plot(ENUcorrected2(1,:),ENUcorrected2(2,:),'*c','DisplayName','Mahalanobis') 207 | end 208 | if gpsResults 209 | hold on 210 | plot(ENUGPS(1),ENUGPS(2),'o','DisplayName','Truth Start') 211 | hold on 212 | 213 | plot(ENUGPS(1,end),ENUGPS(2,end),'*','DisplayName','Truth End') 214 | end 215 | xlabel('E(m) ') 216 | ylabel('N(m) ') 217 | legend('show'); 218 | if gpsResults() 219 | h(7)=figure; 220 | subplot(311),plot(tTimu(1:end-1,1)-tTimu(1),gpsLongerXYZ(1,:)-xyz(1,:),'DisplayName','x_{err}') 221 | ylabel('x (m)') 222 | xlabel('time (sec)') 223 | subplot(312),plot(tTimu(1:end-1,1)-tTimu(1),gpsLongerXYZ(2,:)-xyz(2,:),'DisplayName','y_{err}') 224 | ylabel('y (m)') 225 | xlabel('time (sec)') 226 | subplot(313),plot(tTimu(1:end-1,1)-tTimu(1),gpsLongerXYZ(3,:)-xyz(3,:),'DisplayName','z_{err}') 227 | ylabel('z (m)') 228 | xlabel('time (sec)') 229 | end 230 | h(8)=figure; 231 | hold off 232 | 233 | loyolagray = 1/255*[150,150,150]; 234 | for ii=1:1500:length(ENU(1:100000)) 235 | d(ii) = pdist([ENU(1,ii),ENU(2,ii);ENU3P(1,ii),ENU3P(2,ii)],'euclidean'); 236 | b(ii) = pdist([ENU(2,ii),ENU(3,ii);ENU3P(2,ii),ENU3P(3,ii)],'euclidean'); 237 | coords(:,:,ii)=ellipse3D(d(ii),b(ii),ENU(1,ii),ENU(2,ii),ENU(3,ii),300,pi/2,0,insAtt(3,ii)); 238 | % plot3([coords]') 239 | % ELLIPSE(rx,ry,x0,y0,z0,Nb, pitch,roll,yaw) adds an on the XY plane 240 | % ellipse with semimajor axis of rx, a semimajor axis of radius ry centered 241 | % at the point x0,y0,z0 and a pose in 3D space defined by roll, pitch, and 242 | % yaw angles 243 | % plot(x,y,'k')% xxx(kkk)=x; 244 | hold on 245 | plot3(coords(1,:,ii),coords(2,:,ii),coords(3,:,ii),'Color',loyolagray) 246 | % legend 247 | end 248 | 249 | plot3(ENU(1,:),ENU(2,:),ENU(3,:),'.r','DisplayName','Filter Estimate') 250 | if gpsResults 251 | hold on 252 | % plot3(ENU3P(1,:),ENU3P(2,:),ENU3P(3,:),'--k','DisplayName','Covariance Ellipse') 253 | % hold on 254 | % plot3(ENU_3P(1,:),ENU_3P(2,:),ENU_3P(3,:),'--k') 255 | % hold on 256 | plot3(ENUGPS(1,:),ENUGPS(2,:),ENUGPS(3,:),'.-k','DisplayName','GPS Solution') 257 | end 258 | grid on 259 | xlabel('East (m)') 260 | ylabel('North (m)') 261 | zlabel('Up (m)') 262 | view(-73,40) 263 | hold off 264 | legend({'Filter Estimate','GPS Solution', 'Covariance Ellipse'}) 265 | 266 | if contactAngle() 267 | h(9)=figure; 268 | fill( [tTimu(1:end-1,1)'-tTimu(1) fliplr(tTimu(1:end-1,1)'-tTimu(1))], [3*sqrt(psig1)+x_gamma(1,:) fliplr(-3*sqrt(psig1)+x_gamma(1,:))], 'b','FaceAlpha',.25,'DisplayName','3\sigma Covariance Hull' ); 269 | hold on 270 | plot(tTimu(1:end-1,1)-tTimu(1),(x_gamma(1,:)),'.b','DisplayName','FrontWheelContactAngle_{est}') 271 | hold on 272 | plot(tTimu(1:end-1,1)-tTimu(1),(insAtt(2,:)*180/pi),'-k','DisplayName','INSPitchAngle_{est}') 273 | hold on 274 | fill( [tTimu(1:end-1,1)'-tTimu(1) fliplr(tTimu(1:end-1,1)'-tTimu(1))], [3*sqrt(psig2)+x_gamma(2,:) fliplr(-3*sqrt(psig2)+x_gamma(2,:))], 'r','FaceAlpha',.25,'DisplayName','3\sigma Covariance Hull' ); 275 | hold on 276 | plot(tTimu(1:end-1,1)-tTimu(1),(x_gamma(2,:)),'.r','DisplayName','RearWheelContactAngle_{est}') 277 | 278 | grid on 279 | ylabel('Angle (deg)') 280 | xlabel('Time (s)') 281 | legend('show','Location','northwest') 282 | end 283 | 284 | % [envHigh, envLow] = envelope(ENUGPS(1,:),16,'peak'); 285 | % envMean = (envHigh+envLow)/2; 286 | % 287 | % plot(tTodom-min(tTodom),ENUGPS(1,1:length(tTodom)), ... 288 | % tTodom-min(tTodom),envHigh(1,1:length(tTodom)), ... 289 | % tTodom-min(tTodom),envMean(1,1:length(tTodom)), ... 290 | % tTodom-min(tTodom),envLow(1,1:length(tTodom))) -------------------------------------------------------------------------------- /PostProcess_Matlab/figureGeneration.m: -------------------------------------------------------------------------------- 1 | for ss=1:length(insLLH) 2 | xyz(:,ss)=llh2xyz(insLLH(:,ss)); 3 | if gpsResults() 4 | gpsLongerXYZ(:,ss)=llh2xyz(gpsLonger(:,ss)); 5 | end 6 | xyz_3P(:,ss)=llh2xyz([insLLH(1,ss)-sig7(ss);insLLH(2,ss)-sig8(ss);insLLH(3,ss)-sig9(ss)]); 7 | xyz3P(:,ss)=llh2xyz([insLLH(1,ss)+sig7(ss);insLLH(2,ss)+sig8(ss);insLLH(3,ss)+sig9(ss)]); 8 | ENU(:,ss)=xyz2enu(xyz(:,ss),xyzInit); 9 | ENU3P(:,ss)=xyz2enu(xyz3P(:,ss),xyzInit); 10 | ENU_3P(:,ss)=xyz2enu(xyz_3P(:,ss),xyzInit); 11 | 12 | xyz3(:,ss)=llh2xyz(insLLHCorr(:,ss)); 13 | xyz3_3P(:,ss)=llh2xyz([insLLHCorr(1,ss)-sig7(ss);insLLHCorr(2,ss)-sig8(ss);insLLHCorr(3,ss)-sig9(ss)]); 14 | xyz33P(:,ss)=llh2xyz([insLLHCorr(1,ss)+sig7(ss);insLLHCorr(2,ss)+sig8(ss);insLLHCorr(3,ss)+sig9(ss)]); 15 | ENU3(:,ss)=xyz2enu(xyz3(:,ss),xyzInit); 16 | ENU33P(:,ss)=xyz2enu(xyz33P(:,ss),xyzInit); 17 | ENU3_3P(:,ss)=xyz2enu(xyz3_3P(:,ss),xyzInit); 18 | end 19 | for ss2=1:length(LLHcorrected) 20 | xyzCorrected(:,ss2)=llh2xyz(LLHcorrected(:,ss2)); 21 | xyzCorrected_3P(:,ss2)=llh2xyz([LLHcorrected(1,ss2)-sig7(ss2);LLHcorrected(2,ss2)-sig8(ss2);LLHcorrected(3,ss2)-sig9(ss2)]); 22 | xyzCorrected3P(:,ss2)=llh2xyz([LLHcorrected(1,ss2)+sig7(ss2);LLHcorrected(2,ss2)+sig8(ss2);LLHcorrected(3,ss2)+sig9(ss2)]); 23 | ENUcorrected(:,ss2)=xyz2enu(xyzCorrected(:,ss2),xyzInit); 24 | ENU3Pcorrected(:,ss2)=xyz2enu(xyzCorrected3P(:,ss2),xyzInit); 25 | ENU_3Pcorrected(:,ss2)=xyz2enu(xyzCorrected_3P(:,ss2),xyzInit); 26 | end 27 | if odomUpdate 28 | for ss3=1:length(LLHcorrected2) 29 | xyzCorrected2(:,ss3)=llh2xyz(LLHcorrected2(:,ss3)); 30 | ENUcorrected2(:,ss3)=xyz2enu(xyzCorrected2(:,ss3),xyzInit); 31 | end 32 | 33 | for ss4=1:length(LLHcorrected1) 34 | xyzCorrected1(:,ss4)=llh2xyz(LLHcorrected1(:,ss4)); 35 | ENUcorrected1(:,ss4)=xyz2enu(xyzCorrected1(:,ss4),xyzInit); 36 | end 37 | end 38 | if gpsResults() 39 | gpsLongerXYZ(:,1)=xyzInit; 40 | RMSE_east=sqrt(mean((xyz(1,:)-gpsLongerXYZ(1,:)).^2)); %R Mean Squared Error 41 | RMSE_north=sqrt(mean((xyz(2,:)-gpsLongerXYZ(2,:)).^2)); %R Mean Squared Error 42 | RMSE_up=sqrt(mean((xyz(3,:)-gpsLongerXYZ(3,:)).^2)); %R Mean Squared Error 43 | end 44 | %% 45 | xy_err=ENU33P(1:2,:)-ENU3(1:2,:); 46 | for i=1:length(xy_err) 47 | dPx(1,i)=sqrt(xy_err(1:2,i)'*xy_err(1:2,i)); 48 | end 49 | %% 50 | if gpsResults 51 | errXY=xyz(1:2,:)-gpsLongerXYZ(1:2,:); 52 | for i=1:length(errXY) 53 | dPx2(1,i)=sqrt(errXY(1:2,i)'*errXY(1:2,i)); 54 | end 55 | 56 | medHor=median(dPx2) 57 | stdHor=std(dPx2) 58 | maxHor=max(dPx2) 59 | 60 | RMSE_e=sqrt(mean((xyz(1,:)-gpsLongerXYZ(1,:)).^2)) %R Mean Squared Error 61 | RMSE_n=sqrt(mean((xyz(2,:)-gpsLongerXYZ(2,:)).^2)) %R Mean Squared Error 62 | RMSE_u=sqrt(mean((xyz(3,:)-gpsLongerXYZ(3,:)).^2)) %R Mean Squared Error 63 | end 64 | figGen 65 | % hold off 66 | % ellipsegen 67 | % hampel(mahala,50) 68 | % figure; 69 | % plot(slipR) 70 | % ylim([-1,1]) 71 | % figure; 72 | % plot(slipF) 73 | % ylim([-1,1]) 74 | % hold off 75 | % gpsLongerXYZ(1,1)=856507.849400000; 76 | 77 | % h(8)=figure; 78 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU33P(1,:)-ENU3(1,:),'-.k','DisplayName','3sigma') 79 | % hold on 80 | % plot(tTimu(1:end-1,1)-tTimu(1),abs(xyz3(1,:)-gpsLongerXYZ(1,:)),'','DisplayName','abs(lln-gps)') 81 | % ylabel('x_{err}(m)') 82 | % xlabel('time') 83 | % legend('show'); 84 | % h(9)=figure; 85 | % plot(tTimu(1:end-1,1)-tTimu(1),ENU33P(2,:)-ENU3(2,:),'-.k','DisplayName','3sigma') 86 | % hold on 87 | % plot(tTimu(1:end-1,1)-tTimu(1),abs(xyz3(2,:)-gpsLongerXYZ(2,:)),'','DisplayName','abs(lln-gps)') 88 | % ylabel('y_{err}(m)') 89 | % xlabel('time') 90 | % legend('show'); 91 | % figure;plot(tTimu(1:end-1,1)-tTimu(1),dPx) 92 | % figure;plot(tTimu(1:end-1,1)-tTimu(1),dPx2) 93 | -------------------------------------------------------------------------------- /PostProcess_Matlab/geocentricLattitude.m: -------------------------------------------------------------------------------- 1 | function geocLat = geocentricLatitude(lat) 2 | f= 1/298.257223560; 3 | t = (1 - f)^2; 4 | geocLat= atan2(t*sin(lat), cos(lat)); 5 | end 6 | -------------------------------------------------------------------------------- /PostProcess_Matlab/geocradius.m: -------------------------------------------------------------------------------- 1 | function radius = geocradius(geoLat) 2 | 3 | sinlam=sin(geoLat); 4 | % hard coded for Earth WGS84 5 | f= 1/298.257223560; 6 | R=6378137; 7 | radius = sqrt(( R.^2 )./( 1 + (1/(( 1 - f ).^2) - 1).*sinlam.^2 )); 8 | -------------------------------------------------------------------------------- /PostProcess_Matlab/getQins.m: -------------------------------------------------------------------------------- 1 | %inputs(F21,CbnPlus, T_p_rn) 2 | 3 | function [Qins]=getQins(F21,Cb2nPlus, insLLH, R_N,R_E,dt) 4 | T_rn_p=[1.0/(R_N+insLLH(3)),0,0; 5 | 0,1.0/((R_E+insLLH(3))*cos(insLLH(1))),0; 6 | 0,0,-1]; 7 | Ts=dt;%1/200 8 | g= 9.80665; 9 | deg2rad = pi/180; 10 | 11 | %% TODO -- check below values for the specific IMU::ADIS 16488 12 | %eq 14.83,84 pg 592 13 | % init_accel_bias = 0; 14 | % init_gyro_bias = 0.7*deg2rad; 15 | % sig_accel_inRun = (3.2e-6*g); % m/s 16 | % sig_gyro_inRun = 0.8*4.848e-6 ; %rad/s 17 | % 18 | % sig_VRW = .008*sqrt(Ts); %m/s 19 | % sig_ARW = .09*2.909e-4*sqrt(Ts); %rad 20 | % sig_gyro_inRun = 1.6*pi/180/3600;% | sig_bgd::rad/s 21 | % sig_ARW = .3*(pi/180)*sqrt(3600)/3600;% | sig_rg ::rad/s 22 | % sig_accel_inRun = (0.1e-3*g); % | sig_bad::m/s^2 23 | % sig_VRW = 0.029*sqrt(3600)/3600;% | sig_ra ::m/s^2 !! 24 | sig_gyro_inRun = 1.6*pi/180/3600;% | sig_bgd::rad/s 25 | sig_ARW = .1*(pi/180)*sqrt(3600)/3600;% | sig_rg ::rad/s 26 | sig_accel_inRun = (3.2e-6*g); % | sig_bad::m/s^2 27 | sig_VRW = 0.008*sqrt(3600)/3600;% | sig_ra ::m/s^2 !! 28 | % following 14.2.6 of Groves pp 592 29 | Srg= (sig_ARW^2)*Ts; 30 | Sra= (sig_VRW^2)*Ts; 31 | 32 | Sbad=(sig_accel_inRun^2)/Ts; 33 | Sbgd=(sig_gyro_inRun^2)/Ts; 34 | 35 | %% 36 | 37 | %Use Navigation Frame, gamma=n, Q , eq 14.81 38 | Q11=(Srg*Ts+(1/3)*Sbgd*Ts^3)*eye(3); % Checked 39 | 40 | Q21=((1/2)*Srg*Ts^2+(1/4)*Sbgd*Ts^4)*F21; %Checked 41 | 42 | Q12=Q21';%Checked 43 | 44 | Q31=((1/3)*Srg*Ts^3+(1/5)*Sbgd*Ts^5)*T_rn_p*F21; %Checked 45 | 46 | Q13=Q31'; %Checked 47 | 48 | Q14=zeros(3); %Checked 49 | 50 | Q15=(1/2)*Sbgd*Ts^2*Cb2nPlus; %Checked 51 | 52 | Q22=(Sra*Ts+(1/3)*Sbad*Ts^3)*eye(3)+((1/3)*Srg*Ts^3+(1/5)*Sbgd*Ts^5)*(F21*F21'); %Checked 53 | 54 | Q32=((1/2)*Sra*Ts^2+(1/4)*Sbad*Ts^4)*T_rn_p+((1/4)*Srg*Ts^4+(1/6)*Sbgd*Ts^6)*T_rn_p*(F21*F21'); %Checked 55 | 56 | Q23=Q32'; %Checked 57 | 58 | Q24=(1/2)*Sbad*Ts^2*Cb2nPlus; %Checked 59 | 60 | Q25=(1/3)*Sbgd*Ts^3*F21*Cb2nPlus; %Checked 61 | 62 | Q33=((1/3)*Sra*Ts^3 + (1/5)*Sbad*Ts^5)*(T_rn_p*T_rn_p)+((1/5)*Srg*Ts^5+(1/7)*Sbgd*Ts^7)*T_rn_p*(F21*F21')*T_rn_p; %Checked 63 | 64 | Q34=(1/3)*Sbad*Ts^3*T_rn_p*Cb2nPlus; %Checked 65 | 66 | Q35=(1/4)*Sbgd*Ts^4*T_rn_p*F21*Cb2nPlus; %Checked 67 | 68 | Q41=zeros(3); %Checked 69 | 70 | Q42=(1/2)*Sbad*Ts^2*(Cb2nPlus)';%Cnb=Cbn' %Checked 71 | 72 | Q43=Q34'; %Checked 73 | 74 | Q44=Sbad*Ts*eye(3); %Checked 75 | 76 | Q45=zeros(3); %Checked 77 | 78 | Q51=(1/2)*Sbgd*Ts^2*(Cb2nPlus)'; %Checked 79 | 80 | Q52=(1/3)*Sbgd*Ts^3*F21'*(Cb2nPlus)'; %Checked 81 | 82 | Q53=Q35'; %Checked 83 | 84 | Q54=zeros(3); %Checked 85 | 86 | Q55=Sbgd*Ts*eye(3); %Checked 87 | 88 | 89 | 90 | Qins=[Q11, Q12, Q13, Q14, Q15; 91 | Q21, Q22, Q23, Q24, Q25; 92 | Q31, Q32, Q33, Q34, Q35; 93 | Q41, Q42, Q43, Q44, Q45; 94 | Q51, Q52, Q53, Q54, Q55]; 95 | 96 | 97 | 98 | % P=blkdiag(eye(3)*(init_accel_bias^2)*Ts,eye(3)*(init_gyro_bias^2)*Ts); 99 | % 100 | % noiseChar = [Srg,Sra,Sbad,Sbgd]; 101 | % 102 | % Pgyro=diag((ARW).^2); -------------------------------------------------------------------------------- /PostProcess_Matlab/gpsconversion.m: -------------------------------------------------------------------------------- 1 | for ss=1:length(gpsECEF.x) 2 | llhGPS(:,ss)=xyz2llh([gpsECEF.x(ss);gpsECEF.y(ss);gpsECEF.z(ss)]); 3 | end 4 | for ss=1:length(gpsECEF.x) 5 | ENUGPS(:,ss)=xyz2enu([gpsECEF.x(ss);gpsECEF.y(ss);gpsECEF.z(ss)],[gpsECEF.x(1);gpsECEF.y(1);gpsECEF.z(1)]); 6 | end 7 | -------------------------------------------------------------------------------- /PostProcess_Matlab/gravity.m: -------------------------------------------------------------------------------- 1 | function g = gravity(lat,height) 2 | %GRAVITY Plumb-bob gravity calculation. 3 | % 4 | % g = gravity(lat,height) 5 | % 6 | % INPUTS 7 | % lat = latitude in radians 8 | % height = height above the ellipsoid in meters 9 | % 10 | % OUTPUTS 11 | % g = gravity in meters/second 12 | % 13 | 14 | % REFERENCES 15 | % Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION 16 | % TECHNOLOGY, Peter Peregrinus Ltd. on behalf of the Institution 17 | % of Electrical Engineers, London, 1997. 18 | % 19 | % M. & S. Braasch 4-98 20 | % Copyright (c) 1998 by GPSoft LLC 21 | % All Rights Reserved. 22 | % 23 | 24 | if nargin<2,error('insufficient number of input arguments'),end 25 | 26 | h = height; 27 | [Rm,Rp] = radicurv(lat); 28 | Ro = sqrt(Rp*Rm); % mean earth radius of curvature 29 | g0 = 9.780318*( 1 + 5.3024e-3*(sin(lat))^2 - 5.9e-6*(sin(2*lat))^2 ); 30 | if h >= 0, 31 | g = g0/( (1 + h/Ro)^2 ); 32 | end 33 | if h < 0, 34 | g = g0*(1 + h/Ro); 35 | end, 36 | if h < -Ro, 37 | error(' Input altitude is less than -(earth radius) ') 38 | end, 39 | -------------------------------------------------------------------------------- /PostProcess_Matlab/insErrorStateModel_LNF.m: -------------------------------------------------------------------------------- 1 | %R_E should be R_EPlus 2 | %Cbn should be CbnPlus 3 | function [STM] = insErrorStateModel_LNF(R_E,R_N,insLLH,insVel,dt,Cb2nPlus,omega_ie,omega_n_in,f_ib_b) 4 | 5 | Z=zeros(3); 6 | I=eye(3); 7 | 8 | rGeoCent=geocradius(geocentricLattitude(insLLH(1)*180/pi)); 9 | g0=9.780318*( 1 + 5.3024e-3*(sin(insLLH(1)))^2 - 5.9e-6*(sin(2*insLLH(1)))^2 ); 10 | 11 | %% Eq:14.65 12 | F12_1=[0, -1.0/(R_E+insLLH(3)), 0]; % Checked 13 | F12_2=[1.0/(R_N+insLLH(3)), 0, 0]; % Checked 14 | F12_3=[0, tan(insLLH(1))/(R_E+insLLH(3)), 0]; %Checked 15 | %% Eq:14.66 16 | F13_1=[omega_ie*sin(insLLH(1)), 0, insVel(2)/(R_E+insLLH(3))^2]; %Checked 17 | F13_2=[0, 0, -insVel(1)/(R_N+insLLH(3))^2]; %Checked 18 | F13_3=[omega_ie*cos(insLLH(1))+insVel(2)/((R_E+insLLH(3))*cos(insLLH(1))^2), 0, -insVel(2)*tan(insLLH(1))/(R_E+insLLH(3))^2]; %Checked 19 | %% Eq:14.68 20 | F22_1=[insVel(3)/(R_N+insLLH(3)), -((2.0)*insVel(2)*tan(insLLH(1))/(R_E+insLLH(3)))-(2.0)*omega_ie*sin(insLLH(1)), insVel(1)/(R_N+insLLH(3))]; %Checked 21 | F22_2=[(insVel(2)*tan(insLLH(1))/(R_E+insLLH(3)))+(2.0)*omega_ie*sin(insLLH(1)), (insVel(1)*tan(insLLH(1))+insVel(3))/(R_E+insLLH(3)), (insVel(2)/(R_E+insLLH(3)))+(2.0)*omega_ie*cos(insLLH(1))]; %Checked 22 | F22_3=[(-2.0)*insVel(1)/(R_N+insLLH(3)), (-2.0)*(insVel(2)/(R_E+insLLH(3)))-(2.0)*omega_ie*cos(insLLH(1)), 0]; %Checked 23 | %% Eq:14.69 24 | F23_1=[-(((insVel(2)^2)*sec(insLLH(1))^2)/(R_E+insLLH(3)))-(2*insVel(2)*omega_ie*cos(insLLH(1))), 0, (((insVel(2)^2)*(tan(insLLH(1))))/((R_E+insLLH(3))^2))-((insVel(1)*insVel(3))/(R_N+insLLH(3))^2)]; %Checked 25 | F23_2=[((insVel(1)*insVel(2)*sec(insLLH(1))^2)/(R_E+insLLH(3)))+2*insVel(1)*omega_ie*cos(insLLH(1))-2*insVel(3)*omega_ie*sin(insLLH(1)), 0, -((insVel(1)*insVel(2)*tan(insLLH(1))+insLLH(2)*insLLH(3))/(R_E+insLLH(3))^2)]; %Checked 26 | F23_3=[(2.0)*insVel(2)*omega_ie*sin(insLLH(1)), 0, (((insVel(2)^2)/(R_E+insLLH(3))^2)+((insVel(1)^2)/(R_N+insLLH(3))^2)-((2*g0)/(rGeoCent)))]; %Checked 27 | %% Eq:14.70 28 | F32_1=[(1.0)/(R_N+insLLH(3)), 0, 0]; %Checked 29 | F32_2=[0, (1.0)/((R_E+insLLH(3))*cos(insLLH(1))), 0]; %Checked 30 | F32_3=[0, 0, -1.0];%Checked 31 | %% Eq:14.71 32 | F33_1=[0, 0, -insVel(1)/(R_N+insLLH(3))^2]; %Checked 33 | F33_2=[(insVel(2)*sin(insLLH(1)))/((R_E+insLLH(3))*cos(insLLH(1))^2), 0, -(insVel(2))/(((R_E+insLLH(3))^2)*cos(insLLH(1)))]; %Checked 34 | F33_3=[0, 0, 0]; %Checked 35 | 36 | F11= -skewsymm(omega_n_in); %Omega_n_in %Eq:14.64 37 | F12= [F12_1; F12_2; F12_3]; 38 | F13= [F13_1; F13_2; F13_3]; 39 | F21= -skewsymm(Cb2nPlus*(f_ib_b)'); %Eq:14.67 40 | F22= [F22_1; F22_2; F22_3]; 41 | F23= [F23_1; F23_2; F23_3]; 42 | F32= [F32_1; F32_2; F32_3]; 43 | F33= [F33_1; F33_2; F33_3]; 44 | 45 | PHI11 = I+F11*dt; 46 | PHI12 = F12*dt; 47 | PHI13 = F13*dt; 48 | PHI15 = Cb2nPlus*dt; 49 | 50 | PHI21 = F21*dt; 51 | PHI22 = I+F22*dt; 52 | PHI23 = F23*dt; 53 | PHI24 = Cb2nPlus*dt; 54 | 55 | PHI32 = F32*dt; 56 | PHI33 = I+F33*dt; 57 | 58 | %Eq:14.72 59 | STM = [PHI11 PHI12 PHI13 Z PHI15; 60 | PHI21 PHI22 PHI23 PHI24 Z; 61 | Z PHI32 PHI33 Z Z; 62 | Z Z Z I Z; 63 | Z Z Z Z I]; 64 | 65 | end 66 | -------------------------------------------------------------------------------- /PostProcess_Matlab/llh2xyz.m: -------------------------------------------------------------------------------- 1 | function xyz = llh2xyz(llh) 2 | %LLH2XYZ Convert from latitude, longitude and height 3 | % to ECEF cartesian coordinates. WGS-84 4 | % 5 | % xyz = LLH2XYZ(llh) 6 | % 7 | % llh(1) = latitude in radians 8 | % llh(2) = longitude in radians 9 | % llh(3) = height above ellipsoid in meters 10 | % 11 | % xyz(1) = ECEF x-coordinate in meters 12 | % xyz(2) = ECEF y-coordinate in meters 13 | % xyz(3) = ECEF z-coordinate in meters 14 | 15 | % Reference: Understanding GPS: Principles and Applications, 16 | % Elliott D. Kaplan, Editor, Artech House Publishers, 17 | % Boston, 1996. 18 | % 19 | % M. & S. Braasch 10-96 20 | % Copyright (c) 1996 by GPSoft 21 | % All Rights Reserved. 22 | 23 | phi = llh(1); 24 | lambda = llh(2); 25 | h = llh(3); 26 | 27 | a = 6378137.0000; % earth semimajor axis in meters 28 | b = 6356752.3142; % earth semiminor axis in meters 29 | e = sqrt (1-(b/a).^2); 30 | 31 | sinphi = sin(phi); 32 | cosphi = cos(phi); 33 | coslam = cos(lambda); 34 | sinlam = sin(lambda); 35 | tan2phi = (tan(phi))^2; 36 | tmp = 1 - e*e; 37 | tmpden = sqrt( 1 + tmp*tan2phi ); 38 | 39 | x = (a*coslam)/tmpden + h*coslam*cosphi; 40 | 41 | y = (a*sinlam)/tmpden + h*sinlam*cosphi; 42 | 43 | tmp2 = sqrt(1 - e*e*sinphi*sinphi); 44 | z = (a*tmp*sinphi)/tmp2 + h*sinphi; 45 | 46 | xyz=[x;y;z]; 47 | -------------------------------------------------------------------------------- /PostProcess_Matlab/nonHolonomic.m: -------------------------------------------------------------------------------- 1 | function [insAtt_new,insVel_new,insLLH_new,x_err_new,P_new]= nonHolonomic(insVel_old,insAtt_old,insLLH_old,x_err_old,P_old,omega_n_ie,omega_b_ib,A) 2 | 3 | Cn2bUnc = eulr2dcm(insAtt_old); 4 | %% Measurement Matrix members -- 5 | H32=-[0,1,0]*Cn2bUnc; 6 | H42=-[0,0,1]*Cn2bUnc; 7 | %% Measurement Innovation w/o time -- INS eq:16.42 -- 8 | z31=-[0,1,0]*(Cn2bUnc*insVel_old- skewsymm(omega_b_ib)*[-A,0,0]'); 9 | z41=-[0,0,1]*(Cn2bUnc*insVel_old- skewsymm(omega_b_ib)*[-A,0,0]'); 10 | %% Measurement Matrix -- Checked 11 | H=[zeros(1,3), H32, zeros(1,3), zeros(1,3) zeros(1,3); 12 | zeros(1,3), H42, zeros(1,3), zeros(1,3) zeros(1,3)]; 13 | %% Measurement Noise Covariance Matrix 14 | R= [0.05,0; 15 | 0,0.05]; 16 | %% Kalman Gain 17 | K=P_old*H'*inv(H*P_old*H'+R); 18 | 19 | %% Update state estimation 20 | x_err_new=(x_err_old+K*([z31;z41]-H*x_err_old));% 21 | insAtt_new=dcm2eulr((eye(3)-skewsymm(x_err_new(1:3)))*Cn2bUnc'); 22 | insVel_new=(insVel_old-x_err_new(4:6)); 23 | insLLH_new=insLLH_old-x_err_new(7:9); 24 | x_err_new(1:9)=zeros(1,9); 25 | %% Update P matrix 26 | P_new=(eye(15) - K*H)*P_old*(eye(15)-K*H)' + K*R*K'; 27 | 28 | end -------------------------------------------------------------------------------- /PostProcess_Matlab/odom.m: -------------------------------------------------------------------------------- 1 | wheel_radius=0.1651;%meters 2 | wheelbase=0.555; %meters 3 | i=1; 4 | for i=2:min(length(rearLeftPos),length(lin_x)) 5 | DeltaPosRearRightWheel=rearRightPos(i)-rearRightPos(i-1); 6 | DeltaPosRearLeftWheel=rearLeftPos(i)-rearLeftPos(i-1); 7 | 8 | DeltaPosRearWheel(i)=(DeltaPosRearLeftWheel+DeltaPosRearRightWheel)/2; 9 | rearVel(i)=(rearRightVel(i)+rearLeftVel(i))*wheel_radius/2; %lin_x from odom 10 | 11 | % DeltaPosFrontWheel(i)=(DeltaPosFrontLeftWheel+DeltaPosFrontRightWheel)/2; 12 | frontVel(i)=(frontRightVel(i)+frontLeftVel(i))*wheel_radius/2; %lin_x from odom 13 | 14 | DeltaPosFrontRightWheel=frontRightPos(i)-frontRightPos(i-1); 15 | DeltaPosFrontLeftWheel=frontLeftPos(i)-frontLeftPos(i-1); 16 | DeltaPosRightWheel(i)=(DeltaPosFrontRightWheel+DeltaPosRearRightWheel)/2; %rad 17 | DeltaPosLeftWheel(i)=(DeltaPosFrontLeftWheel+DeltaPosRearLeftWheel)/2; %rad 18 | heading(i)=(DeltaPosRightWheel(i)-DeltaPosLeftWheel(i))/(wheelbase); %ang_z from odom 19 | headRate(i)=((rearLeftVel(i)+frontLeftVel(i))/2-(rearRightVel(i)+frontRightVel(i))/2)*wheel_radius/(0.545); 20 | headRateF(i)=(frontLeftVel(i)-frontRightVel(i))/(0.272); 21 | end 22 | -------------------------------------------------------------------------------- /PostProcess_Matlab/odomMeasGen.m: -------------------------------------------------------------------------------- 1 | % z and H values are integrated for IMU only below. When the Odometry 2 | % update is available (tTimu(i)>=tTodom(kk)) integrated values will be 3 | % updated by odometry measurements, used in the filter and destroyed. 4 | Cn2bPlus=Cb2nPlus'; 5 | omega_b_ie=Cn2bPlus*omega_n_ie; 6 | omega_b_ei=-omega_b_ie; 7 | omega_b_eb=omega_b_ei+omega_b_ib'; 8 | %% Measurement Matrix -- integration part for INS -- eq 16.48 9 | H11= H11+ [1,0,0]*Cn2bPlus*skewsymm(insVel(:,i))*dtIMU; 10 | H12= H12+ [1,0,0]*Cn2bPlus*dtIMU; 11 | H21= H21+ sin(insAtt(2,i))*[0,cos(insAtt(1,i)),sin(insAtt(1,i))]*Cn2bPlus*dtIMU; 12 | H31= H31+ [0,1,0]*Cn2bPlus*skewsymm(insVel(:,i))*dtIMU; 13 | H32= H32+ [0,1,0]*Cn2bPlus*dtIMU; 14 | H41= H41+ [0,0,1]*Cn2bPlus*skewsymm(insVel(:,i))*dtIMU; 15 | H42= H42+ [0,0,1]*Cn2bPlus*dtIMU; 16 | %% Measurement Innovation -- integration part for INS -- eq 16.42 17 | z11=z11+[1,0,0]*(Cn2bPlus*insVel(:,i) + skewsymm(omega_b_eb)*[-A,0,0]')*dtIMU; 18 | z21=z21+cos(insAtt(2,i))*dtIMU; 19 | z31=z31+[0,1,0]*(Cn2bPlus*insVel(:,i) + skewsymm(omega_b_eb)*[-A,0,0]')*dtIMU; 20 | z41=z41+[0,0,1]*(Cn2bPlus*insVel(:,i) + skewsymm(omega_b_eb)*[-A,0,0]')*dtIMU; -------------------------------------------------------------------------------- /PostProcess_Matlab/odomUpt.m: -------------------------------------------------------------------------------- 1 | function [insAtt_new,insVel_new,insLLH_new,x_err_new,P_new,postFit]= odomUpt(insVel_old,insAtt_old,insLLH_old,x_err_old,P_old,insAtt_older,heading_old,dt_odom,rearVel,headRate,s_or,H11,H12,H21,H31,H32,H41,H42,z11,z21,z31,z41,T_r,sigma_or_L,sigma_or_R,sigma_cmc,s_delta_or) 2 | 3 | Cn2bUnc = eulr2dcm(insAtt_old); 4 | %% Measurement Matrix members -- 5 | H11=-H11/dt_odom; 6 | H12=-H12/dt_odom; 7 | H21=H21*(insAtt_old(3)-insAtt_older(3))/dt_odom^2; 8 | H31=-H31/dt_odom; 9 | H32=-H32/dt_odom; 10 | H24=-(cos(insAtt_old(2))*[0,0,1]*Cn2bUnc')/dt_odom; 11 | H41=-H41/dt_odom; 12 | H42=-H42/dt_odom; 13 | 14 | %% Measurement Innovation -- Odometry eq:16.42 -- 15 | z1_odom=rearVel*(1-s_or); 16 | z2_odom=(headRate)*(1-s_or)-((z11/dt_odom)/T_r)*s_delta_or; 17 | 18 | %% Measurement Innovation w/o time -- INS eq:16.42 -- 19 | z1_ins=z11; %Checked 20 | z2_ins=((insAtt_old(3)-heading_old))*z21; 21 | if abs(z2_ins) > 0.5 22 | disp('Heading spike! errorCorrected') 23 | z2_ins=0; 24 | end 25 | z3_ins=z31; %Checked 26 | %% Measurement Innovation -- (z= Odom - INS) with zero cross-track eq:16.42 and z speed 27 | z11=z1_odom-z1_ins/dt_odom; 28 | z21=z2_odom-z2_ins/dt_odom^2; 29 | z31=0.0-z31/dt_odom; 30 | z41=0.0-z41/dt_odom; 31 | 32 | %% Measurement Matrix -- Checked 33 | H=[H11, H12, zeros(1,3) zeros(1,3) zeros(1,3); 34 | H21, zeros(1,3) zeros(1,3) H24, zeros(1,3); 35 | H31, H32, zeros(1,3), zeros(1,3) zeros(1,3); 36 | H41, H42, zeros(1,3), zeros(1,3) zeros(1,3)]; 37 | %% Measurement Noise Covariance Matrix 38 | R= [0.5, 0.5, 0,0; 39 | 1/T_r,-1/T_r, 0,0; 40 | 0, 0, 1,0; 41 | 0,0,0,1]* ... 42 | [sigma_or_L^2, 0, 0,0; 43 | 0, sigma_or_R^2, 0,0; 44 | 0, 0, sigma_cmc^2,0; 45 | 0,0,0,0.05^2]* ... 46 | [0.5, 1/T_r, 0,0; 47 | 0.5, -1/T_r, 0,0; 48 | 0, 0, 1,0; 49 | 0,0,0,1]; 50 | 51 | %% Kalman Gain 52 | K=P_old*H'*inv(H*P_old*H'+R); 53 | 54 | % C_deltaz=H*P_old*H'+R; 55 | % CC=sqrt(diag(C_deltaz)) 56 | % sqrt(C_deltaz) 57 | 58 | %% Update state estimation 59 | x_err_new=(x_err_old+K*([z11;z21;z31;z41]-H*x_err_old));%;/sqrt(diag(C_deltaz)); 60 | postFit.z=[z11;z21;z31;z41]; 61 | postFit.H=H; 62 | insAtt_new=dcm2eulr((eye(3)-skewsymm(x_err_new(1:3)))*Cn2bUnc'); 63 | insVel_new=(insVel_old-x_err_new(4:6)); 64 | insLLH_new=insLLH_old-x_err_new(7:9); 65 | x_err_new(1:9)=zeros(1,9); 66 | 67 | %% Update P matrix 68 | P_new=(eye(15) - K*H)*P_old*(eye(15)-K*H)' + K*R*K'; 69 | 70 | % counter=counter+1; 71 | % kk=kk+1; 72 | end -------------------------------------------------------------------------------- /PostProcess_Matlab/odomslip.m: -------------------------------------------------------------------------------- 1 | odompostfit(:,odomUptCount)=postFitOdom.z-postFitOdom.H*x_err; 2 | S=postFitOdom.H*P*postFitOdom.H'+[0.00045,0,0,0;0,0.1152,0,0;0,0,0.0025,0;0,0,0,0.0025]; 3 | chisq(:,odomUptCount)=odompostfit(:,odomUptCount)'*inv(S)*odompostfit(:,odomUptCount); 4 | mahala(:,odomUptCount)=sqrt(odompostfit(:,odomUptCount)'*inv(S)*odompostfit(:,odomUptCount)); 5 | % if mahala(1,odomUptCount)>5 6 | 7 | % if mahala(1,odomUptCount)>5 8 | % 9 | % insAtt(:,i)=insAtt_old; 10 | % insVel(:,i)=insVel_old; 11 | % insLLH(:,i)=insLLH_old; 12 | % P=P_old; 13 | % x_err=x_err_old; 14 | % LLHcorrected2(:,cttr2)=insLLH(:,i); 15 | % cttr2=cttr2+1; 16 | % end 17 | slipR(:,odomUptCount)=(rearVel(odomUptCount)-sqrt(insVel(1,i)^2+insVel(2,i)^2))/rearVel(odomUptCount); 18 | if slipR(:,odomUptCount) < -50 19 | slipR (:,odomUptCount) = 0; 20 | end 21 | slipF(:,odomUptCount)=(frontVel(odomUptCount)-sqrt(insVel(1,i)^2+insVel(2,i)^2))/rearVel(odomUptCount); 22 | if slipF(:,odomUptCount) < -50 23 | slipF (:,odomUptCount) = 0; 24 | end 25 | if odomUptCount > 25 26 | if abs(slipF(1,odomUptCount))>0.3 27 | 28 | LLHcorrected1(:,cttr0)=insLLH(:,i); 29 | dadas(cttr0)=std(mahala(1,(odomUptCount-25):odomUptCount)); 30 | cttr0=cttr0+1; 31 | 32 | if mahala(1,odomUptCount)>5 33 | 34 | % insAtt(:,i)=insAtt_old; 35 | % insVel(:,i)=insVel_old; 36 | % insLLH(:,i)=insLLH_old; 37 | % P=P_old; 38 | % x_err=x_err_old; 39 | LLHcorrected2(:,cttr2)=insLLH(:,i); 40 | cttr2=cttr2+1; 41 | end 42 | 43 | end 44 | end -------------------------------------------------------------------------------- /PostProcess_Matlab/radicurv.m: -------------------------------------------------------------------------------- 1 | function [Rm,Rp,R_es_e] = radicurv(lat) 2 | %RADICURV Calculate meridian and prime radii of curvature. 3 | % 4 | % [Rm,Rp] = radicurv(lat) 5 | % 6 | % INPUTS 7 | % lat = geodetic latitude in radians 8 | % 9 | % OUTPUTS 10 | % Rm = meridian radius of curvature in meters 11 | % Rp = prime radius of curvature in meters 12 | % 13 | % NOTE 14 | % In some literature Rp is also referred to as the 'normal' 15 | % or 'transverse' radius of curvature 16 | % 17 | % REFERENCES 18 | % Brockstein, A. and J. Kouba, "Derivation of Free Inertial, General 19 | % Wander Azimuth Mechanization Equations," Litton Systems, Inc., 20 | % Guidance and Control Systems Division, Woodland Hills, California, 21 | % June 1969, Revised June 1981. 22 | % 23 | % Kayton, M. and W. Fried, AVIONICS NAVIGATION SYSTEMS, 2nd edition, 24 | % John Wiley & Sons, New York, 1997. 25 | % 26 | % Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION 27 | % TECHNOLOGY, Peter Peregrinus Ltd. on behalf of the Institution 28 | % of Electrical Engineers, London, 1997. 29 | % 30 | % M. & S. Braasch 3-98 31 | % Copyright (c) 1997-98 by GPSoft LLC 32 | % All Rights Reserved. 33 | % 34 | 35 | if nargin<1,error('insufficient number of input arguments'),end 36 | 37 | EQUA_RADIUS = 6378137.0; % equatorial radius of the earth; WGS-84 38 | ECCENTRICITY = 0.0818191908426; % eccentricity of the earth ellipsoid 39 | 40 | e2 = ECCENTRICITY^2; 41 | den = 1-e2*(sin(lat))^2; 42 | Rm = (EQUA_RADIUS*(1-e2))/( (den)^(3/2) ); 43 | Rp = EQUA_RADIUS/( sqrt(den) ); 44 | 45 | 46 | Top = EQUA_RADIUS*(sqrt(((1-e2)^2)+((sin(lat))^2)+((cos(lat))^2))); 47 | Bottom = sqrt((1-e2*((sin(lat))^2))); 48 | R_es_e = Top/Bottom; 49 | -------------------------------------------------------------------------------- /PostProcess_Matlab/skewsymm.m: -------------------------------------------------------------------------------- 1 | function S = skewsymm(vect) 2 | %SKEWSYMM Convert a vector into corresponding 3 | % skew symmetric matrix. 4 | % 5 | % S = skewsymm(vect) 6 | % 7 | % INPUTS 8 | % vect = 3 element vector 9 | % vect(1) = sigma_x; 10 | % vect(2) = sigma_y; 11 | % vect(3) = sigma_z; 12 | % 13 | % OUTPUTS 14 | % S = [ 0 -sigma_z sigma_y; 15 | % sigma_z 0 -sigma_x; 16 | % -sigma_y sigma_x 0 ] 17 | % 18 | 19 | % M. & S. Braasch 1-98 20 | % Copyright (c) 1997-98 by GPSoft 21 | % All Rights Reserved. 22 | % 23 | 24 | if nargin<1,error('insufficient number of input arguments'),end 25 | 26 | sigma_x = vect(1); 27 | sigma_y = vect(2); 28 | sigma_z = vect(3); 29 | % 30 | S = [ 0 -sigma_z sigma_y; 31 | sigma_z 0 -sigma_x; 32 | -sigma_y sigma_x 0 ]; 33 | 34 | 35 | -------------------------------------------------------------------------------- /PostProcess_Matlab/smoothback.m: -------------------------------------------------------------------------------- 1 | 2 | function [P_s_old,x_s_old] = smoothback(P_plus_old,P_minus_new,STM_old,x_plus_old,x_minus_new,x_s_new,P_s_new,STM_s_new) 3 | STM_s_new=STM_old-eye(15).*x_s_new; 4 | AA=P_plus_old*(STM_s_new)'*pinv(P_minus_new); %old = k-1, new = k 5 | x_s_old=x_plus_old+AA*(x_s_new-x_minus_new); 6 | P_s_old=P_plus_old+AA*(P_s_new-P_minus_new)*AA'; 7 | end 8 | -------------------------------------------------------------------------------- /PostProcess_Matlab/wheelTerrainContactAngle.m: -------------------------------------------------------------------------------- 1 | function[x_new,P_new]=wheelTerrainContactAngle(p,pr,vf,vb,x_old,P_old) 2 | l=0.5; %m distance between wheels 3 | 4 | a=l*pr/vf; 5 | b=vb/vf; 6 | 7 | if (2*a^2+2*b^2+2*a^2*b^2-a^4-b^4-1)>=0 8 | h=(1/(2*a))*sqrt(2*a^2+2*b^2+2*a^2*b^2-a^4-b^4-1); 9 | 10 | gamma1=p-acos(h);%contact angle 1 11 | gamma2=acos(h/b)+p; %contact angle 2 12 | if isreal(gamma1) || isreal(gamma2) 13 | 14 | dyz=[1, -(pr^4/16 - vb^4 + 2*vb^2*vf^2 - vf^4)/(pr^2*vf^3*((- pr^4/16 + (pr^2*vb^2)/2 + (pr^2*vf^2)/2 - vb^4 + 2*vb^2*vf^2 - vf^4)/vf^4)^(1/2)*((pr^2/4 - vb^2 + vf^2)^2/(pr^2*vf^2))^(1/2)), -(- pr^4/16 + (pr^2*vb^2)/2 - vb^4 + vf^4)/(pr*vf^4*((- pr^4/16 + (pr^2*vb^2)/2 + (pr^2*vf^2)/2 - vb^4 + 2*vb^2*vf^2 - vf^4)/vf^4)^(1/2)*((pr^2/4 - vb^2 + vf^2)^2/(pr^2*vf^2))^(1/2)), (4*vb*(pr^2/4 - vb^2 + vf^2))/(pr*vf^3*((- pr^4/16 + (pr^2*vb^2)/2 + (pr^2*vf^2)/2 - vb^4 + 2*vb^2*vf^2 - vf^4)/vf^4)^(1/2)*((4*(pr^2/4 - vb^2 + vf^2)^2)/(pr^2*vf^2))^(1/2)); 15 | 1, (pr^4/16 - vb^4 + 2*vb^2*vf^2 - vf^4)/(pr^2*vb*vf^2*((- pr^4/16 + (pr^2*vb^2)/2 + (pr^2*vf^2)/2 - vb^4 + 2*vb^2*vf^2 - vf^4)/vf^4)^(1/2)*((pr^2/4 + vb^2 - vf^2)^2/(pr^2*vb^2))^(1/2)), -(pr^2 + 4*vb^2 - 4*vf^2)/(2*pr*vb*vf*((- pr^4/16 + (pr^2*vb^2)/2 + (pr^2*vf^2)/2 - vb^4 + 2*vb^2*vf^2 - vf^4)/vf^4)^(1/2)*((pr^2/4 + vb^2 - vf^2)^2/(pr^2*vb^2))^(1/2)), (- pr^4/16 + (pr^2*vf^2)/2 + vb^4 - vf^4)/(pr*vb^2*vf^2*((- pr^4/16 + (pr^2*vb^2)/2 + (pr^2*vf^2)/2 - vb^4 + 2*vb^2*vf^2 - vf^4)/vf^4)^(1/2)*((pr^2/4 + vb^2 - vf^2)^2/(pr^2*vb^2))^(1/2))]; 16 | 17 | R=diag([3^2, 3^2, 0.05^2, 0.05^2]); 18 | 19 | K=P_old*inv(P_old+dyz*R*dyz'); 20 | 21 | x_new=x_old+K*([gamma1;gamma2]-x_old); 22 | 23 | P_new=(eye(2)- K)*P_old; 24 | else 25 | x_new=x_old; 26 | P_new=P_old; 27 | end 28 | else 29 | x_new=x_old; 30 | P_new=P_old; 31 | end 32 | -------------------------------------------------------------------------------- /PostProcess_Matlab/xyz2enu.m: -------------------------------------------------------------------------------- 1 | function enu = xyz2enu(xyz,orgxyz) 2 | %XYZ2ENU Convert from WGS-84 ECEF cartesian coordinates to 3 | % rectangular local-level-tangent ('East'-'North'-Up) 4 | % coordinates. 5 | % 6 | % enu = XYZ2ENU(xyz,orgxyz) 7 | % 8 | % INPUTS 9 | % xyz(1) = ECEF x-coordinate in meters 10 | % xyz(2) = ECEF y-coordinate in meters 11 | % xyz(3) = ECEF z-coordinate in meters 12 | % 13 | % orgxyz(1) = ECEF x-coordinate of local origin in meters 14 | % orgxyz(2) = ECEF y-coordinate of local origin in meters 15 | % orgxyz(3) = ECEF z-coordinate of local origin in meters 16 | % 17 | % OUTPUTS 18 | % enu: Column vector 19 | % enu(1,1) = 'East'-coordinate relative to local origin (meters) 20 | % enu(2,1) = 'North'-coordinate relative to local origin (meters) 21 | % enu(3,1) = Up-coordinate relative to local origin (meters) 22 | 23 | % Reference: Alfred Leick, GPS Satellite Surveying, 2nd ed., 24 | % Wiley-Interscience, John Wiley & Sons, 25 | % New York, 1995. 26 | 27 | 28 | if nargin<2,error('insufficient number of input arguments'),end 29 | tmpxyz = xyz; 30 | tmporg = orgxyz; 31 | if size(tmpxyz) ~= size(tmporg), tmporg=tmporg'; end, 32 | difxyz = tmpxyz - tmporg; 33 | [m,n] = size(difxyz); if m=0 59 | long = temp; 60 | elseif (x < 0) & (y >= 0) 61 | long = pi + temp; 62 | else 63 | long = temp - pi; 64 | end 65 | 66 | llh(1) = lat; 67 | llh(2) = long; 68 | llh(3) = height; 69 | 70 | -------------------------------------------------------------------------------- /PostProcess_Matlab/zeroUpd.m: -------------------------------------------------------------------------------- 1 | %% ZARU - Zero Angular Rate Update 2 | % Zero angular rate may be assumed whenever the vehicle is stationary 3 | % so ZARU may be performed whenever a ZUPT performed. 4 | % ----------------------------------------------------- 5 | % When odometry is available it may be used to trigger a ZARU when the vehicle is 6 | % stationary. 7 | % ----------------------------------------------------- 8 | % ZARU may also be performed when the vehicle is moving at a 9 | % constant heading. This may be detected by comparing three parameters with 10 | % thresholds: 11 | % the standard deviation of the recent yaw-rate gyro measurements, 12 | % the standard deviation of the steering-angle commands, 13 | % yaw rate obtained from differential odometry. 14 | % ----------------------------------------------------- 15 | % 16 | function [insVel_new,insAtt_new,insLLH_new,x_err_new,P_new,postFit] = zeroUpd(insVel_old,insAtt_old,insLLH_old,x_err_old,P_old,omega_b_ib) 17 | Cn2b_zaru= eulr2dcm(insAtt_old); 18 | z_zaru=-omega_b_ib'; 19 | z_zupt=-insVel_old; 20 | H_zaru=[zeros(3), zeros(3), zeros(3), zeros(3), -eye(3)]; 21 | H_zupt=[zeros(3), -eye(3), zeros(3), zeros(3), zeros(3)]; 22 | H_tot=[H_zaru;H_zupt]; 23 | R_zaru=[0.01^2, 0.0, 0.0; 24 | 0.0, 0.01^2, 0.0; 25 | 0.0, 0.0, 0.0025^2]; 26 | R_zupt=[0.02^2, 0.0, 0.0; 27 | 0.0, 0.02^2, 0.0; 28 | 0.0, 0.0, 0.02^2]; 29 | 30 | R_tot=eye(6).*[diag(R_zaru);diag(R_zupt)]; 31 | K_zaru=P_old*H_tot'*inv(H_tot*P_old*H_tot'+R_tot); 32 | x_err_new=x_err_old+K_zaru*([z_zaru;z_zupt]-H_tot*x_err_old); 33 | postFit.z=z_zaru; 34 | postFit.H=H_zaru; 35 | insAtt_new=dcm2eulr((eye(3)-skewsymm(x_err_new(1:3)))*Cn2b_zaru'); 36 | insVel_new=insVel_old-x_err_new(4:6); 37 | insLLH_new=insLLH_old-x_err_new(7:9); 38 | x_err_new(1:9)=zeros(1,9); 39 | P_new=(eye(15) - K_zaru*H_tot)*P_old*(eye(15)-K_zaru*H_tot)' + K_zaru*R_tot*K_zaru'; 40 | end 41 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

2 | Architecture 3 |

4 | 5 | ## Overview 6 | 7 | **Author: Cagri Kilic
8 | Affiliation: [WVU NAVLAB](https://navigationlab.wvu.edu/)
9 | Maintainer: Cagri Kilic, cakilic@mix.wvu.edu** 10 | 11 | Improved Planetary Rover Inertial Navigation and Wheel Odometry Performance through Periodic Use of Zero-Type Constraint 12 | 13 |

14 | Architecture 15 |

16 | 17 | This repository provides a localization algorithm that compensates for the high likelihood of odometry errors by providing a reliable localization solution that leverages nonholonomic vehicle constraints as well as state-aware pseudo-measurements (e.g., zero velocity and zero angular rate) updates during periodic stops. 18 | 19 | 20 | **Keywords:** localization, dead-reckoning, zero-velocity, non-holonomic 21 | 22 | 23 | ## Citation 24 | 25 | If you find this library useful, please cite the following publication: 26 | 27 | * Cagri Kilic, Jason N. Gross, Nicholas Ohi, Ryan Watson, Jared Strader, Thomas Swiger, Scott Harper, and Yu Gu: **Improved Planetary Rover Inertial Navigation and Wheel Odometry Performance through Periodic Use of Zero-Type Constraints**. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019. ([arXiv:1906.08849](https://arxiv.org/pdf/1906.08849.pdf)) 28 | 29 | @inproceedings{Kilic2019, 30 | author = {Kilic, Cagri and Gross,Jason N. and Ohi,Nicholas and Watson, Ryan and Strader, Jared and Swiger, Thomas and Harper, Scott and Gu, Yu}, 31 | booktitle = {2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 32 | title = {{Improved Planetary Rover Inertial Navigation and Wheel Odometry Performance through Periodic Use of Zero-Type Constraints}}, 33 | year = {2019}, 34 | pages={552-559}, 35 | doi={10.1109/IROS40897.2019.8967634}, 36 | ISSN={2153-0858}, 37 | publisher = {IEEE}, 38 | 39 | } 40 | 41 | 42 | ## IROS Figures 43 | 44 | To generate the same figures in the paper, use [v.1.0](https://github.com/wvu-navLab/CLN/tree/v1.0) and simply run MainRun.m 45 | -------------------------------------------------------------------------------- /core_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(core_nav) 3 | 4 | set (CMAKE_CXX_STANDARD 11) 5 | 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | geometry_utils 10 | parameter_utils 11 | geometry_msgs 12 | sensor_msgs 13 | tf2_ros 14 | tf 15 | std_msgs 16 | message_generation 17 | ) 18 | 19 | generate_messages( 20 | DEPENDENCIES 21 | std_msgs 22 | ) 23 | 24 | catkin_package( 25 | INCLUDE_DIRS include 26 | LIBRARIES ${PROJECT_NAME} 27 | CATKIN_DEPENDS 28 | roscpp 29 | geometry_utils 30 | parameter_utils 31 | geometry_msgs 32 | sensor_msgs 33 | tf2_ros 34 | tf 35 | ) 36 | 37 | include_directories( 38 | include 39 | ${catkin_INCLUDE_DIRS} 40 | ) 41 | 42 | link_directories( 43 | ${catkin_LIBRARY_DIRS} 44 | ) 45 | 46 | add_library(${PROJECT_NAME} src/CoreNav.cpp) 47 | target_link_libraries(${PROJECT_NAME} 48 | ${catkin_LIBRARIES} 49 | ) 50 | 51 | add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}.cpp) 52 | target_link_libraries(${PROJECT_NAME}_node 53 | ${PROJECT_NAME} 54 | ${catkin_LIBRARIES} 55 | ) 56 | -------------------------------------------------------------------------------- /core_navigation/config/init_params.yaml: -------------------------------------------------------------------------------- 1 | bias_a: 2 | x: 0.0 #-0.132490269370 3 | y: 0.0 # -0.056562486375 4 | z: 0.0 #-9.808054251834 5 | bias_g: 6 | x: 0.0 #0.000665131773 7 | y: 0.0 #0.000308961740 8 | z: 0.0 #0.000451598618 9 | -------------------------------------------------------------------------------- /core_navigation/config/parameters.yaml: -------------------------------------------------------------------------------- 1 | init: 2 | position: 3 | x: 0.691949695095547 4 | y: -1.395750757746275 5 | z: 312.2643980319930 6 | covx: 1.218469679146834e-14 7 | covy: 1.218469679146834e-14 8 | covz: 0.010000000000000 9 | velocity: 10 | x: 0.0 11 | y: 0.0 12 | z: 0.0 13 | covx: 4.000000000000001e-07 14 | covy: 4.000000000000001e-07 15 | covz: 4.000000000000001e-07 16 | 17 | orientation: 18 | x: 0.0 19 | y: 0.0 20 | z: 0.0 21 | covx: 1.218469679146834e-06 22 | covy: 1.218469679146834e-06 23 | covz: 4.873878716587337e-06 24 | bias: 25 | accel: 26 | x: 7.549135895545244e-04 27 | y: 0.001805886718248 28 | z: 0.002395586011438 29 | gyro: 30 | x: 8.674066138832567e-05 31 | y: 1.005391303275586e-04 32 | z: 8.919962404601004e-05 33 | ecef: 34 | x: 8.565078494000000e+05 35 | y: -4.842978088500000e+06 36 | z: 4.047980191900000e+06 37 | 38 | imu: 39 | publish_hz: 50 40 | sensor_pub_rate: 125 41 | topic: /imu/dataAdis 42 | noise: 43 | sig_gyro_inRun : 7.757018897752577e-06 44 | sig_ARW : 2.908882086657216e-05 45 | sig_accel_inRun : 3.139200000000000e-05 46 | sig_VRW : 1.333333333333333e-04 47 | 48 | 49 | odo: 50 | topic: /husky_velocity_controller/odom 51 | updates: 52 | do_zupt: false 53 | do_zaru: false 54 | 55 | 56 | joint: 57 | topic: /joint_states 58 | 59 | frames: 60 | frame_id_out: map 61 | frame_id_fixed: odom 62 | frame_id_imu: imu_link 63 | frame_id_odo: odom_link 64 | 65 | wheel: 66 | T_r_: 0.125 67 | s_or_: 0.0 68 | s_delta_or_: 0.0 69 | -------------------------------------------------------------------------------- /core_navigation/include/core_navigation/CoreNav.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Redistribution and use in source and binary forms, with or without 3 | * modification, are permitted provided that the following conditions are 4 | * met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright 7 | * notice, this list of conditions and the following disclaimer. 8 | * 9 | * 2. Redistributions in binary form must reproduce the above 10 | * copyright notice, this list of conditions and the following 11 | * disclaimer in the documentation and/or other materials provided 12 | * with the distribution. 13 | * 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | * POSSIBILITY OF SUCH DAMAGE. 29 | * 30 | * Please contact the author(s) of this library if you have any questions. 31 | * Authors: Cagri, Ryan 32 | */ 33 | 34 | #ifndef core_navigation_H 35 | #define core_navigation_H 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | #include 50 | #include 51 | 52 | class CoreNav { 53 | public: 54 | 55 | typedef sensor_msgs::Imu ImuData; 56 | typedef nav_msgs::Odometry OdoData; 57 | typedef sensor_msgs::JointState JointData; 58 | typedef geometry_msgs::PoseStamped PoseData; 59 | 60 | typedef Eigen::VectorXd Vector; 61 | typedef Eigen::MatrixXd Matrix; 62 | 63 | typedef Eigen::Matrix Vector3; 64 | typedef Eigen::Matrix Vector4; 65 | typedef Eigen::Matrix Vector6; 66 | typedef Eigen::Matrix Vector13; 67 | typedef Eigen::Matrix Vector15; 68 | 69 | typedef Eigen::Matrix Matrix3; 70 | Matrix3 CbnMinus; 71 | Matrix3 eye3=Eigen::Matrix3d::Identity(); 72 | Matrix3 zeros3=Eigen::Matrix3d::Zero(3,3); 73 | 74 | CoreNav::Vector13 odo; 75 | CoreNav::Vector6 imu; 76 | CoreNav::Vector4 joint; 77 | 78 | CoreNav(); 79 | ~CoreNav(); 80 | 81 | // Calls LoadParameters and RegisterCallbacks. Fails on failure of either. 82 | bool Initialize(const ros::NodeHandle& n); 83 | 84 | private: 85 | // Node initialization 86 | bool Init(const ros::NodeHandle& n); 87 | bool LoadParameters(const ros::NodeHandle& n); 88 | bool RegisterCallbacks(const ros::NodeHandle& n); 89 | 90 | // Publish estimated states. 91 | void PublishStates(const CoreNav::Vector3& states, const ros::Publisher& pub); 92 | 93 | void ImuCallback(const ImuData& imu_data_); 94 | void OdoCallback(const OdoData& odo_data_); 95 | void JointCallBack(const JointData& joint_data_); 96 | 97 | void Update(const CoreNav::Vector13& odo); 98 | void Propagate(const CoreNav::Vector6& imu, const CoreNav::Vector4& joint); 99 | 100 | void NonHolonomic(const CoreNav::Vector3 vel, const CoreNav::Vector3 att, const CoreNav::Vector3 llh, CoreNav::Vector15 errorStates, Eigen::MatrixXd P, CoreNav::Vector3 omega_b_ib); 101 | 102 | //Zero vel update 103 | void zupt(const CoreNav::Vector3 vel, const CoreNav::Vector3 att, const CoreNav::Vector3 llh, CoreNav::Vector15 errorStates, Eigen::MatrixXd P); 104 | // Zero ang. update 105 | void zaru(const CoreNav::Vector3 vel, const CoreNav::Vector3 att, const CoreNav::Vector3 llh, CoreNav::Vector15 errorStates, Eigen::MatrixXd P, const CoreNav::Vector3 omega_b_ib); 106 | 107 | CoreNav::Vector3 calc_gravity(const double latitude, const double height); 108 | CoreNav::Matrix3 skew_symm(const CoreNav::Vector3 vec); 109 | CoreNav::Matrix3 eul_to_dcm(double phi, double theta, double psi); 110 | CoreNav::Vector3 dcm_to_eul(CoreNav::Matrix3 dcm); 111 | CoreNav::Vector3 llh_to_enu(const double latitude, const double longitude, const double height); 112 | CoreNav::Matrix insErrorStateModel_LNF(double R_EPlus, double R_N, CoreNav::Vector3 insLLHPlus, CoreNav::Vector3 insVelPlus, double dt,CoreNav::Matrix3 CbnPlus, double omega_ie, CoreNav::Vector3 omega_n_in,Vector3 f_ib_b,double gravity); 113 | CoreNav::Matrix calc_Q(double R_N, double R_E, CoreNav::Vector3 insLLHPlus, double dt, CoreNav::Matrix3 CbnPlus, CoreNav::Vector3 f_ib_b); 114 | 115 | CoreNav::Vector6 getImuData(const ImuData& imu_data_); 116 | CoreNav::Vector4 getJointData(const JointData &joint_data_); 117 | CoreNav::Vector13 getOdoData(const OdoData& odo_data_); 118 | 119 | // The node's name. 120 | std::string name_; 121 | 122 | // Subscriber 123 | ros::Subscriber imu_sub_; 124 | ros::Subscriber odo_sub_; 125 | ros::Subscriber joint_sub_; 126 | 127 | // Publisher. 128 | ros::Publisher position_pub_, velocity_pub_, attitude_pub_, enu_pub_; 129 | tf::TransformBroadcaster transformed_states_tf_broad; 130 | 131 | OdoData odo_data_; 132 | OdoData odo_data_prev_; 133 | ImuData imu_data_; 134 | JointData joint_data_; 135 | 136 | bool has_odo_ = false; 137 | bool has_joint_ = false; 138 | bool has_imu_ = false; 139 | bool first_odo_ = true; 140 | bool first_joint_ = true; 141 | bool first_imu_ = true; 142 | 143 | // Most recent time stamp for publishers. 144 | ros::Time stamp_; 145 | 146 | // Coordinate frames. 147 | std::string frame_id_out_; 148 | std::string frame_id_imu_; 149 | std::string frame_id_odo_; 150 | std::string frame_id_fixed_; 151 | 152 | // update rate [hz] 153 | unsigned int publish_hz_; 154 | unsigned int sensor_pub_rate_; 155 | 156 | // sub. topics 157 | std::string imu_topic_; 158 | std::string odo_topic_; 159 | std::string joint_topic_; 160 | 161 | // For initialization. 162 | bool initialized_; 163 | 164 | // Filter vars. 165 | int num_states_ = 15; 166 | CoreNav::Vector15 error_states_; // {pos., vel, att, ba, bg} 167 | CoreNav::Vector3 ba_; 168 | CoreNav::Vector3 bg_; 169 | CoreNav::Vector3 ins_att_, ins_vel_, ins_pos_, ins_enu_; 170 | CoreNav::Vector4 Z_; 171 | CoreNav::Matrix P_, Q_, STM_; 172 | Eigen::Matrix R_; 173 | Eigen::Matrix R_zupt; 174 | Eigen::Matrix R_zaru; 175 | Eigen::Matrix R_holo; 176 | Eigen::Matrix K_; 177 | Eigen::Matrix K_zupt; 178 | Eigen::Matrix K_zaru; 179 | Eigen::Matrix K_holo; 180 | Eigen::Matrix H_; 181 | Eigen::Matrix H_zupt; 182 | Eigen::Matrix H_zaru; 183 | Eigen::Matrix H_holo; 184 | Eigen::Matrix z_holo; 185 | 186 | CoreNav::Vector3 H11_, H12_, H21_, H31_, H32_, H24_, H41_, H42_; 187 | double z11_, z21_, z31_, z41_; 188 | double rearVel_, headRate_, T_r_, s_or_, s_delta_or_; 189 | 190 | CoreNav::Vector3 omega_b_ib_, omega_b_ib_prev_, omega_n_ie_; 191 | CoreNav::Vector3 f_ib_b_, f_ib_b_prev_, omega_n_en_, omega_n_in_, grav_; 192 | CoreNav::Matrix3 Omega_b_ib_, Omega_n_ie_, Omega_n_en_; 193 | 194 | // imu noise params 195 | double sig_gyro_inRun_, sig_ARW_, sig_accel_inRun_, sig_VRW_; 196 | // filter noise params 197 | double position_noise_, attitude_noise_, velocity_noise_, bias_noise_; 198 | 199 | // initial pose 200 | double init_x, init_y, init_z, init_vx, init_vy, init_vz, psiEst; 201 | double init_roll, init_pitch, init_yaw, sigma_x, sigma_y; 202 | double sigma_z, sigma_vx, sigma_vy, prev_stamp_, up_time_; 203 | double sigma_vz, sigma_roll, sigma_pitch, sigma_yaw; 204 | double init_ba_x, init_ba_y, init_ba_z, init_bg_x, init_bg_y, init_bg_z; 205 | double init_bias_a_x, init_bias_a_y, init_bias_a_z, init_bias_g_x, init_bias_g_y, init_bias_g_z; 206 | double init_ecef_x,init_ecef_y,init_ecef_z; 207 | double imu_stamp_curr_, imu_stamp_prev_, odo_stamp_curr_, odo_stamp_prev_; 208 | double joint_stamp_curr_, joint_stamp_prev_; 209 | double dt_odo_, dt_imu_; 210 | int count=0; 211 | 212 | }; 213 | #endif 214 | -------------------------------------------------------------------------------- /core_navigation/include/core_navigation/InsConst.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file InsConst.h 3 | * @brief House values that are repeatedly used 4 | * @author Cagri, Ryan 5 | */ 6 | 7 | #pragma once 8 | 9 | namespace INS { 10 | 11 | const double omega_ie = 7.292115e-5; // rotation of Earth in rad/sec 12 | const double Ro = 6378137.0; //WGS84 Equatorial Radius 13 | const double Rp = 6356752.31425; //WGS84 Polar Radius 14 | const double flat= 1.0/298.257223563; // WGS84 Earth flattening 15 | const double ecc = 0.0818191909425; // WGS84 Eccentricity 0.0818191908426 16 | const double t_const = pow((1.0 - flat),2.0); // constant 17 | const double wheel_radius=0.1651; //meters TODO ~0.12 18 | const double wheelbase=0.555; //meters TODO 19 | const double gravity= 9.80665; 20 | const double PI = 3.14159265358979; 21 | 22 | } 23 | -------------------------------------------------------------------------------- /core_navigation/include/core_navigation/InsUtils.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file InsUtils.h 3 | * @brief Tools required to process inertial data 4 | * @author Cagri, Ryan 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | 24 | namespace INS { 25 | 26 | typedef Eigen::VectorXd Vector; 27 | typedef Eigen::MatrixXd Matrix; 28 | 29 | typedef Eigen::Matrix Vector3; 30 | typedef Eigen::Matrix Vector6; 31 | typedef Eigen::Matrix Matrix3; 32 | 33 | Vector3 calc_gravity(const double latitude, const double height); 34 | 35 | Matrix3 skew_symm(const Vector3 vec); 36 | 37 | Matrix3 eul_to_dcm(double phi, double theta, double psi); 38 | 39 | Matrix insErrorStateModel_LNF(double R_EPlus, double R_N, Vector3 insLLHPlus, Vector3 insVelPlus, double dt, Matrix3 CbnPlus, double omega_ie,Vector3 omega_n_in,Vector3 f_ib_b,double gravity); 40 | 41 | Matrix calc_Q(double R_N, double R_E, Vector3 insLLHPlus, double dt, Matrix3 CbnPlus,Vector3 f_ib_b); 42 | 43 | } 44 | -------------------------------------------------------------------------------- /core_navigation/launch/test_online.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | 17 | position: {x: 0.691949695095547, y: -1.395750757746275, z: 312.2643980319930} 18 | velocity: {vx: 0.0, vy: 0.0, vz: 0.0} 19 | orientation: {qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} 20 | 21 | 22 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /core_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | core_nav 4 | 0.1.0 5 | Core Navigation 6 | 7 | Cagri Kilic 8 | Cagri Kilic 9 | 10 | GPLv2 11 | 12 | catkin 13 | 14 | roscpp 15 | geometry_utils 16 | parameter_utils 17 | geometry_msgs 18 | sensor_msgs 19 | nav_msgs 20 | tf2_ros 21 | tf 22 | std_msgs 23 | message_generation 24 | roscpp 25 | geometry_utils 26 | parameter_utils 27 | geometry_msgs 28 | sensor_msgs 29 | nav_msgs 30 | tf2_ros 31 | tf 32 | std_msgs 33 | message_runtime 34 | 35 | -------------------------------------------------------------------------------- /core_navigation/src/InsUtils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file insUtils.cpp 3 | * @brief Tools required to process inertial data 4 | * @authors Cagri, Ryan 5 | */ 6 | 7 | #include 8 | 9 | namespace INS { 10 | 11 | Vector3 calc_gravity(const double latitude, const double height) 12 | { 13 | double e2=pow(ecc,2.0); 14 | double den=1.0-e2*pow(sin(latitude),2.0); 15 | double Rm=Ro*(1.0-e2)/pow(den,(3.0/2.0)); 16 | double Rp=Ro/pow(den,(1.0/2.0)); 17 | double Top=Ro*pow((pow((1.0-e2),2.0)+pow((sin(latitude)),2.0)+pow((cos(latitude)),2.0)),(1.0/2.0)); 18 | double Bottom=pow((1.0-e2*(pow(sin(latitude),(2.0)))),(1.0/2.0)); 19 | double R_es_e=Top/Bottom; 20 | double RO=pow(Rp*Rm,(1.0/2.0)); 21 | double g0=9.780318*(1.0+5.3024e-3*pow(sin(latitude),2.0)-5.9e-6*pow(sin(2*latitude),2.0)); 22 | double gravity; 23 | if(height<0.0) 24 | { 25 | gravity=g0*(1.0+height/RO); 26 | } 27 | else 28 | { 29 | gravity=g0/pow((1.0+height/RO),2.0); 30 | } 31 | Vector3 grav(0.0,0.0,gravity); 32 | return grav; 33 | } 34 | 35 | Matrix3 skew_symm(const Vector3 vec) 36 | { 37 | Matrix3 ss; 38 | ss << 0.0, -1.0*vec(2), vec(1), 39 | vec(2), 0.0, -1.0*vec(0); 40 | -1.0*vec(1), vec(0), 0.0; 41 | 42 | return ss; 43 | } 44 | 45 | Matrix3 eul_to_dcm(double phi, double theta, double psi) 46 | { 47 | Matrix3 CbnMinus; //body2nav 48 | double cpsi = cos(psi); double spsi = sin(psi); 49 | double cthe = cos(theta); double sthe = sin(theta); 50 | double cphi = cos(phi); double sphi = sin(phi); 51 | 52 | Matrix3 c1; //y 53 | c1.row(0)< 35 | #include 36 | 37 | int main(int argc, char** argv){ 38 | ros::init(argc, argv, "inertial propogation"); 39 | ros::NodeHandle n("~"); 40 | 41 | CoreNav CoreNav; 42 | if(!CoreNav.Initialize(n)) { 43 | ROS_ERROR("%s: Failed to initialize the nav. filter.", 44 | ros::this_node::getName().c_str()); 45 | return EXIT_FAILURE; 46 | } 47 | ros::spin(); 48 | 49 | return EXIT_SUCCESS; 50 | } 51 | -------------------------------------------------------------------------------- /core_navigation_demos/doc/adam2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav/60662776a9a935303ab9b7de2d4616f9ef03fbd4/core_navigation_demos/doc/adam2.jpg -------------------------------------------------------------------------------- /core_navigation_demos/doc/architecturev2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav/60662776a9a935303ab9b7de2d4616f9ef03fbd4/core_navigation_demos/doc/architecturev2.jpg -------------------------------------------------------------------------------- /core_navigation_demos/doc/architecturev2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav/60662776a9a935303ab9b7de2d4616f9ef03fbd4/core_navigation_demos/doc/architecturev2.png -------------------------------------------------------------------------------- /core_navigation_demos/doc/corenav4.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav/60662776a9a935303ab9b7de2d4616f9ef03fbd4/core_navigation_demos/doc/corenav4.gif -------------------------------------------------------------------------------- /core_navigation_demos/doc/covLong.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wvu-navLab/corenav/60662776a9a935303ab9b7de2d4616f9ef03fbd4/core_navigation_demos/doc/covLong.jpg -------------------------------------------------------------------------------- /core_navigation_demos/doc/readme.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /core_navigation_demos/readme.md: -------------------------------------------------------------------------------- 1 | # Demos 2 | -------------------------------------------------------------------------------- /geometry_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(geometry_utils) 3 | 4 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") 5 | 6 | find_package(catkin REQUIRED COMPONENTS roscpp) 7 | find_package(Eigen3 REQUIRED) 8 | 9 | catkin_package( 10 | INCLUDE_DIRS 11 | include 12 | ${EIGEN3_INCLUDE_DIR} 13 | LIBRARIES 14 | DEPENDS 15 | Eigen3 16 | ) 17 | 18 | include_directories(include ${EIGEN3_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) 19 | 20 | install(DIRECTORY include/${PROJECT_NAME}/ 21 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 22 | FILES_MATCHING PATTERN "*.h" 23 | ) 24 | 25 | install(DIRECTORY cmake/ 26 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 27 | FILES_MATCHING PATTERN "*.cmake" 28 | ) 29 | 30 | add_executable(test_math tests/test_math.cc) 31 | target_link_libraries(test_math 32 | ${catkin_LIBRARIES} 33 | boost_unit_test_framework 34 | ) 35 | 36 | add_executable(test_base tests/test_base.cc) 37 | target_link_libraries(test_base 38 | ${catkin_LIBRARIES} 39 | boost_unit_test_framework 40 | ) 41 | 42 | add_executable(test_equals tests/test_equals.cc) 43 | target_link_libraries(test_equals 44 | ${catkin_LIBRARIES} 45 | boost_unit_test_framework 46 | ) 47 | 48 | add_executable(test_so3error tests/test_so3error.cc) 49 | target_link_libraries(test_so3error 50 | ${catkin_LIBRARIES} 51 | boost_unit_test_framework 52 | ) 53 | -------------------------------------------------------------------------------- /geometry_utils/cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 79 | 80 | endif(EIGEN3_INCLUDE_DIR) 81 | 82 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/GeometryUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | geometry_utils: Utility library to provide common geometry types and transformations 3 | Copyright (C) 2014 Nathan Michael 4 | 2016 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef GEOMETRY_UTILS_H 22 | #define GEOMETRY_UTILS_H 23 | 24 | #include "Vector2.h" 25 | #include "Vector3.h" 26 | #include "Vector4.h" 27 | #include "Quaternion.h" 28 | #include "Matrix2x2.h" 29 | #include "Matrix3x3.h" 30 | #include "Matrix4x4.h" 31 | #include "Rotation2.h" 32 | #include "Rotation3.h" 33 | #include "Transform2.h" 34 | #include "Transform3.h" 35 | 36 | namespace geometry_utils { 37 | inline double Unroll(double x) { 38 | x = fmod(x, 2.0 * M_PI); 39 | if (x < 0) 40 | x += 2.0 * M_PI; 41 | return x; 42 | } 43 | 44 | inline double Normalize(double x) { 45 | x = fmod(x + M_PI, 2.0 * M_PI); 46 | if (x < 0) 47 | x += 2.0 * M_PI; 48 | return x - M_PI; 49 | } 50 | 51 | inline double S1Distance(double from, double to) { 52 | double result = Unroll(Unroll(to) - Unroll(from)); 53 | if (result > M_PI) 54 | result = -(2.0 * M_PI - result); 55 | return Normalize(result); 56 | } 57 | 58 | inline double Rad2Deg(double angle) { 59 | return angle * 180.0 * M_1_PI; 60 | } 61 | 62 | inline double Deg2Rad(double angle) { 63 | return angle * M_PI / 180.0; 64 | } 65 | 66 | inline Vec3 Rad2Deg(const Vec3& angles) { 67 | return Vec3(Rad2Deg(angles(0)), Rad2Deg(angles(1)), Rad2Deg(angles(2))); 68 | } 69 | 70 | inline Vec3 Deg2Rad(const Vec3& angles) { 71 | return Vec3(Deg2Rad(angles(0)), Deg2Rad(angles(1)), Deg2Rad(angles(2))); 72 | } 73 | 74 | inline Vec3 RToZYX(const Rot3& rot) { 75 | return rot.GetEulerZYX(); 76 | } 77 | 78 | inline Rot3 ZYXToR(const Vec3& angles) { 79 | return Rot3(angles); 80 | } 81 | 82 | inline Rot3 QuatToR(const Quat& quat) { 83 | return Rot3(quat); 84 | } 85 | 86 | inline Quat RToQuat(const Rot3& rot) { 87 | return Quat(Eigen::Quaterniond(rot.Eigen())); 88 | } 89 | 90 | inline double GetRoll(const Rot3& r) { 91 | return r.Roll(); 92 | } 93 | 94 | inline double GetRoll(const Quat& q) { 95 | return Rot3(q).Roll(); 96 | } 97 | 98 | inline double GetPitch(const Rot3& r) { 99 | return r.Pitch(); 100 | } 101 | 102 | inline double GetPitch(const Quat& q) { 103 | return Rot3(q).Pitch(); 104 | } 105 | 106 | inline double GetYaw(const Rot3& r) { 107 | return r.Yaw(); 108 | } 109 | 110 | inline double GetYaw(const Quat& q) { 111 | return Rot3(q).Yaw(); 112 | } 113 | 114 | inline double SO3Error(const Quat& q1, const Quat& q2) { 115 | return Rot3(q1).Error(Rot3(q2)); 116 | } 117 | 118 | inline double SO3Error(const Rot3& r1, const Rot3& r2) { 119 | return r1.Error(r2); 120 | } 121 | 122 | inline Vec3 CartesianToSpherical(const Vec3& v) { 123 | double rho = v.Norm(); 124 | return Vec3(rho, acos(v.Z() / rho), atan2(v.Y(), v.X())); 125 | } 126 | 127 | inline Vec3 SphericalToCartesian(const Vec3& v) { 128 | return Vec3(v(0) * sin(v(1)) * cos(v(2)), v(0) * sin(v(1)) * sin(v(2)), 129 | v(0) * cos(v(1))); 130 | } 131 | 132 | inline Vec3 NEDCartesian(const Vec3& v) { 133 | return Vec3(v(0), -v(1), -v(2)); 134 | } 135 | 136 | } 137 | 138 | #endif 139 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/GeometryUtilsMath.h: -------------------------------------------------------------------------------- 1 | /* 2 | geometry_utils: Utility library to provide common geometry types and transformations 3 | Copyright (C) 2014 Nathan Michael 4 | 2016 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef GEOMETRY_UTILS_MATH_H 22 | #define GEOMETRY_UTILS_MATH_H 23 | 24 | #include 25 | 26 | namespace geometry_utils { 27 | namespace math { 28 | 29 | template inline T cos(T in); 30 | template<> inline float cos(float in) { return ::cosf(in); } 31 | template<> inline double cos(double in) { return ::cos(in); } 32 | 33 | template inline T acos(T in); 34 | template<> inline float acos(float in) { return ::acosf(in); } 35 | template<> inline double acos(double in) { return ::acos(in); } 36 | 37 | template inline T sin(T in); 38 | template<> inline float sin(float in) { return ::sinf(in); } 39 | template<> inline double sin(double in) { return ::sin(in); } 40 | 41 | template inline T asin(T in); 42 | template<> inline float asin(float in) { return ::asinf(in); } 43 | template<> inline double asin(double in) { return ::asin(in); } 44 | 45 | template inline T tan(T in); 46 | template<> inline float tan(float in) { return ::tanf(in); } 47 | template<> inline double tan(double in) { return ::tan(in); } 48 | 49 | template inline T atan(T in); 50 | template<> inline float atan(float in) { return ::atanf(in); } 51 | template<> inline double atan(double in) { return ::atan(in); } 52 | 53 | template inline T fabs(T in); 54 | template<> inline float fabs(float in) { return ::fabsf(in); } 55 | template<> inline double fabs(double in) { return ::fabs(in); } 56 | 57 | template inline T fmin(T v1, T v2); 58 | template<> inline float fmin(float v1, float v2) { return ::fminf(v1, v2); } 59 | template<> inline double fmin(double v1, double v2) { return ::fmin(v1, v2); } 60 | 61 | template inline T fmax(T v1, T v2); 62 | template<> inline float fmax(float v1, float v2) { return ::fmaxf(v1, v2); } 63 | template<> inline double fmax(double v1, double v2) { return ::fmax(v1, v2); } 64 | 65 | template inline T sqrt(T in); 66 | template<> inline float sqrt(float in) { return ::sqrtf(in); } 67 | template<> inline double sqrt(double in) { return ::sqrt(in); } 68 | 69 | template inline T pow(T in, T exp); 70 | template<> inline float pow(float in, float exp) { return ::powf(in, exp); } 71 | template<> inline double pow(double in, double exp) { return ::pow(in, exp); } 72 | 73 | template inline T atan2(T y, T x); 74 | template<> inline float atan2(float y, float x) { return ::atan2f(y, x); } 75 | template<> inline double atan2(double y, double x) { return ::atan2(y, x); } 76 | 77 | template inline T hypot(T x, T y); 78 | template<> inline float hypot(float x, float y) { return ::hypotf(x, y); } 79 | template<> inline double hypot(double x, double y) { return ::hypot(x, y); } 80 | 81 | } 82 | } 83 | #endif 84 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/GeometryUtilsROS.h: -------------------------------------------------------------------------------- 1 | /* 2 | geometry_utils: Utility library to provide common geometry types and transformations 3 | Copyright (C) 2013 Nathan Michael 4 | 2015 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef GEOMETRY_UTILS_ROS_H 22 | #define GEOMETRY_UTILS_ROS_H 23 | 24 | #include "GeometryUtils.h" 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace geometry_utils { 34 | namespace ros { 35 | 36 | inline Vec3 FromROS(const geometry_msgs::Point& p) { 37 | return Vec3(p.x, p.y, p.z); 38 | } 39 | 40 | inline Vec3 FromROS(const geometry_msgs::Point32& p) { 41 | return Vec3(p.x, p.y, p.z); 42 | } 43 | 44 | inline Vec3 FromROS(const geometry_msgs::Vector3& p) { 45 | return Vec3(p.x, p.y, p.z); 46 | } 47 | 48 | inline Quat FromROS(const geometry_msgs::Quaternion& msg) { 49 | return Quat(msg.w, msg.x, msg.y, msg.z); 50 | } 51 | 52 | inline Transform3 FromROS(const geometry_msgs::Pose& msg) { 53 | return Transform3(FromROS(msg.position), QuatToR(FromROS(msg.orientation))); 54 | } 55 | 56 | inline Transform3 FromROS(const geometry_msgs::Transform& msg) { 57 | return Transform3(FromROS(msg.translation), QuatToR(FromROS(msg.rotation))); 58 | } 59 | 60 | inline geometry_msgs::Point ToRosPoint(const Vec2& v) { 61 | geometry_msgs::Point msg; 62 | msg.x = v(0); 63 | msg.y = v(1); 64 | msg.z = 0.0; 65 | 66 | return msg; 67 | } 68 | 69 | inline geometry_msgs::Point ToRosPoint(const Vec3& v) { 70 | geometry_msgs::Point msg; 71 | msg.x = v(0); 72 | msg.y = v(1); 73 | msg.z = v(2); 74 | 75 | return msg; 76 | } 77 | 78 | inline geometry_msgs::Point32 ToRosPoint32(const Vec2& v) { 79 | geometry_msgs::Point32 msg; 80 | msg.x = v(0); 81 | msg.y = v(1); 82 | msg.z = 0.0f; 83 | 84 | return msg; 85 | } 86 | 87 | inline geometry_msgs::Point32 ToRosPoint32(const Vec3& v) { 88 | geometry_msgs::Point32 msg; 89 | msg.x = v(0); 90 | msg.y = v(1); 91 | msg.z = v(2); 92 | 93 | return msg; 94 | } 95 | 96 | inline geometry_msgs::Vector3 ToRosVec(const Vec2& v) { 97 | geometry_msgs::Vector3 msg; 98 | msg.x = v(0); 99 | msg.y = v(1); 100 | msg.z = 0.0; 101 | 102 | return msg; 103 | } 104 | 105 | inline geometry_msgs::Vector3 ToRosVec(const Vec3& v) { 106 | geometry_msgs::Vector3 msg; 107 | msg.x = v(0); 108 | msg.y = v(1); 109 | msg.z = v(2); 110 | 111 | return msg; 112 | } 113 | 114 | inline geometry_msgs::Quaternion ToRosQuat(const Quat& quat) { 115 | geometry_msgs::Quaternion msg; 116 | msg.w = quat.W(); 117 | msg.x = quat.X(); 118 | msg.y = quat.Y(); 119 | msg.z = quat.Z(); 120 | 121 | return msg; 122 | } 123 | 124 | inline geometry_msgs::Pose ToRosPose(const Transform2& trans) { 125 | geometry_msgs::Pose msg; 126 | msg.position = ToRosPoint(trans.translation); 127 | msg.orientation = ToRosQuat(RToQuat(Rot3(trans.rotation))); 128 | 129 | return msg; 130 | } 131 | 132 | inline geometry_msgs::Pose ToRosPose(const Transform3& trans) { 133 | geometry_msgs::Pose msg; 134 | msg.position = ToRosPoint(trans.translation); 135 | msg.orientation = ToRosQuat(RToQuat(trans.rotation)); 136 | 137 | return msg; 138 | } 139 | 140 | inline geometry_msgs::Transform ToRosTransform(const Transform2& trans) { 141 | geometry_msgs::Transform msg; 142 | msg.translation = ToRosVec(trans.translation); 143 | msg.rotation = ToRosQuat(RToQuat(Rot3(trans.rotation))); 144 | 145 | return msg; 146 | } 147 | 148 | inline geometry_msgs::Transform ToRosTransform(const Transform3& trans) { 149 | geometry_msgs::Transform msg; 150 | msg.translation = ToRosVec(trans.translation); 151 | msg.rotation = ToRosQuat(RToQuat(trans.rotation)); 152 | 153 | return msg; 154 | } 155 | 156 | inline geometry_msgs::Quaternion ToRosQuat(const Vec3& angles) { 157 | return ToRosQuat(RToQuat(ZYXToR(angles))); 158 | } 159 | 160 | inline Vec3 RosQuatToZYX(const geometry_msgs::Quaternion& msg) { 161 | return RToZYX(QuatToR(FromROS(msg))); 162 | } 163 | 164 | inline double GetRoll(const geometry_msgs::Quaternion& q) { 165 | return Rot3(FromROS(q)).Roll(); 166 | } 167 | 168 | inline double GetPitch(const geometry_msgs::Quaternion& q) { 169 | return Rot3(FromROS(q)).Pitch(); 170 | } 171 | 172 | inline double GetYaw(const geometry_msgs::Quaternion& q) { 173 | return Rot3(FromROS(q)).Yaw(); 174 | } 175 | 176 | } 177 | } 178 | #endif 179 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/GeometryUtilsSerialization.h: -------------------------------------------------------------------------------- 1 | /* 2 | geometry_utils: Utility library to provide common geometry types and transformations 3 | Copyright (C) 2013 Nathan Michael 4 | 2016 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef GEOMETRY_UTILS_SERIALIZATION_H 22 | #define GEOMETRY_UTILS_SERIALIZATION_H 23 | 24 | #include "GeometryUtils.h" 25 | 26 | #include 27 | #include 28 | 29 | namespace boost { 30 | namespace serialization { 31 | 32 | template 33 | void Serialize(Archive & ar, geometry_utils::Vector2& v, 34 | const unsigned int version) { 35 | ar & v(0); 36 | ar & v(1); 37 | } 38 | 39 | template 40 | void Serialize(Archive & ar, geometry_utils::Vector3& v, 41 | const unsigned int version) { 42 | ar & v(0); 43 | ar & v(1); 44 | ar & v(2); 45 | } 46 | 47 | template 48 | void Serialize(Archive & ar, geometry_utils::Vector4& v, 49 | const unsigned int version) { 50 | ar & v(0); 51 | ar & v(1); 52 | ar & v(2); 53 | ar & v(3); 54 | } 55 | 56 | template 57 | void Serialize(Archive & ar, geometry_utils::Matrix3x3& m, 58 | const unsigned int version) { 59 | for (unsigned int i = 0; i < 3; i++) 60 | for (unsigned int j = 0; j < 3; j++) 61 | ar & m(i, j); 62 | } 63 | 64 | template 65 | void Serialize(Archive & ar, geometry_utils::Transform& t, 66 | const unsigned int version) { 67 | ar & t.translation; 68 | ar & t.rotation; 69 | } 70 | 71 | } 72 | } 73 | #endif 74 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Matrix2x2.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_MATRIX2X2_H 2 | #define GEOMETRY_UTILS_MATRIX2X2_H 3 | 4 | #include "MatrixNxNBase.h" 5 | #include "Vector2.h" 6 | 7 | namespace geometry_utils { 8 | 9 | template 10 | struct Matrix2x2Base : MatrixNxNBase { 11 | Matrix2x2Base() : MatrixNxNBase() {} 12 | Matrix2x2Base(T val) : MatrixNxNBase(val) {} 13 | Matrix2x2Base(const Matrix2x2Base& in) : MatrixNxNBase(in.data) {} 14 | Matrix2x2Base(const boost::array& in) : MatrixNxNBase(in) {} 15 | Matrix2x2Base(T (&in)[2 * 2]) : MatrixNxNBase(in) {} 16 | Matrix2x2Base(const Eigen::Matrix& in) : MatrixNxNBase(in) {} 17 | Matrix2x2Base(const MatrixNxNBase& in) : MatrixNxNBase(in) {} 18 | Matrix2x2Base(const MatrixNxMBase& in) : MatrixNxNBase(in) {} 19 | 20 | Matrix2x2Base(T R11, T R12, T R21, T R22) { 21 | this->data[0] = R11; 22 | this->data[1] = R12; 23 | this->data[2] = R21; 24 | this->data[3] = R22; 25 | } 26 | 27 | virtual inline T Det() const { 28 | T a = this->data[0]; 29 | T b = this->data[1]; 30 | T c = this->data[2]; 31 | T d = this->data[3]; 32 | return (-(b * c) + a * d); 33 | } 34 | 35 | virtual inline MatrixNxNBase Inv() const { 36 | Vector2Base e(SingularValues()); 37 | 38 | T emax = e(0); 39 | T emin = e(1); 40 | 41 | if (emin < std::numeric_limits::denorm_min()) 42 | throw std::runtime_error("Matrix2x2Base: appears singular"); 43 | 44 | if (emax / emin > std::numeric_limits::epsilon()) { 45 | T a = this->data[0]; 46 | T b = this->data[1]; 47 | T c = this->data[2]; 48 | T d = this->data[3]; 49 | 50 | T tmp[4] = {d / (-b * c + a * d), b / (b * c - a * d), 51 | c / (b * c - a * d), a / (-b * c + a * d)}; 52 | return MatrixNxNBase(tmp); 53 | } else 54 | throw std::runtime_error("Matrix2x2Base: appears singular"); 55 | } 56 | 57 | virtual inline Vector2Base SingularValues() const { 58 | T a = this->data[0]; 59 | T b = this->data[1]; 60 | T c = this->data[2]; 61 | T d = this->data[3]; 62 | 63 | T tmp1 = a * a + b * b + c * c + d * d; 64 | T tmp2 = math::sqrt((math::pow(b + c, static_cast(2)) + 65 | math::pow(a - d, static_cast(2))) * 66 | (math::pow(b - c, static_cast(2)) + 67 | math::pow(a + d, static_cast(2)))); 68 | 69 | T e1 = math::sqrt(tmp1 - tmp2) * M_SQRT1_2; 70 | T e2 = math::sqrt(tmp1 + tmp2) * M_SQRT1_2; 71 | 72 | return Vector2Base(e1 > e2 ? e1 : e2, e1 < e2 ? e1 : e2); 73 | } 74 | }; 75 | 76 | inline Matrix2x2Base operator*(const float& lhs, 77 | const Matrix2x2Base& rhs) { 78 | return Matrix2x2Base(rhs * lhs); 79 | } 80 | 81 | inline Matrix2x2Base operator*(const double& lhs, 82 | const Matrix2x2Base& rhs) { 83 | return Matrix2x2Base(rhs * lhs); 84 | } 85 | 86 | typedef Matrix2x2Base Matrix2x2f; 87 | typedef Matrix2x2Base Mat22f; 88 | 89 | typedef Matrix2x2Base Matrix2x2d; 90 | typedef Matrix2x2Base Mat22d; 91 | 92 | typedef Matrix2x2Base Matrix2x2; 93 | typedef Matrix2x2Base Mat22; 94 | 95 | } 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Matrix3x3.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_MATRIX3X3_H 2 | #define GEOMETRY_UTILS_MATRIX3X3_H 3 | 4 | #include "MatrixNxNBase.h" 5 | 6 | namespace geometry_utils { 7 | 8 | template 9 | struct Matrix3x3Base : MatrixNxNBase { 10 | Matrix3x3Base() : MatrixNxNBase() {} 11 | Matrix3x3Base(T val) : MatrixNxNBase(val) {} 12 | Matrix3x3Base(const Matrix3x3Base& in) : MatrixNxNBase(in.data) {} 13 | Matrix3x3Base(const boost::array& in) : MatrixNxNBase(in) {} 14 | Matrix3x3Base(T (&in)[9]) : MatrixNxNBase(in) {} 15 | Matrix3x3Base(const Eigen::Matrix& in) : MatrixNxNBase(in) {} 16 | Matrix3x3Base(const MatrixNxNBase& in) : MatrixNxNBase(in) {} 17 | Matrix3x3Base(const MatrixNxMBase& in) : MatrixNxNBase(in) {} 18 | 19 | Matrix3x3Base(T R11, T R12, T R13, T R21, T R22, T R23, T R31, T R32, T R33) { 20 | this->data[0] = R11; 21 | this->data[1] = R12; 22 | this->data[2] = R13; 23 | this->data[3] = R21; 24 | this->data[4] = R22; 25 | this->data[5] = R23; 26 | this->data[6] = R31; 27 | this->data[7] = R32; 28 | this->data[8] = R33; 29 | } 30 | 31 | inline T Det() const { 32 | T a = this->data[0]; 33 | T b = this->data[1]; 34 | T c = this->data[2]; 35 | T d = this->data[3]; 36 | T e = this->data[4]; 37 | T f = this->data[5]; 38 | T g = this->data[6]; 39 | T h = this->data[7]; 40 | T i = this->data[8]; 41 | return (-(c * e * g) + b * f * g + c * d * h - a * f * h - b * d * i + 42 | a * e * i); 43 | } 44 | 45 | virtual inline MatrixNxNBase Inv() const { 46 | if (math::fabs(Det()) < std::numeric_limits::denorm_min()) 47 | throw std::runtime_error("Matrix3x3Base: appears singular"); 48 | 49 | T a = this->data[0]; 50 | T b = this->data[1]; 51 | T c = this->data[2]; 52 | T d = this->data[3]; 53 | T e = this->data[4]; 54 | T f = this->data[5]; 55 | T g = this->data[6]; 56 | T h = this->data[7]; 57 | T i = this->data[8]; 58 | T tmp[9] = {(f * h - e * i) / (c * e * g - b * f * g - c * d * h + 59 | a * f * h + b * d * i - a * e * i), 60 | (c * h - b * i) / (-(c * e * g) + b * f * g + c * d * h - 61 | a * f * h - b * d * i + a * e * i), 62 | (c * e - b * f) / (c * e * g - b * f * g - c * d * h + 63 | a * f * h + b * d * i - a * e * i), 64 | (f * g - d * i) / (-(c * e * g) + b * f * g + c * d * h - 65 | a * f * h - b * d * i + a * e * i), 66 | (c * g - a * i) / (c * e * g - b * f * g - c * d * h + 67 | a * f * h + b * d * i - a * e * i), 68 | (c * d - a * f) / (-(c * e * g) + b * f * g + c * d * h - 69 | a * f * h - b * d * i + a * e * i), 70 | (e * g - d * h) / (c * e * g - b * f * g - c * d * h + 71 | a * f * h + b * d * i - a * e * i), 72 | (b * g - a * h) / (-(c * e * g) + b * f * g + c * d * h - 73 | a * f * h - b * d * i + a * e * i), 74 | (b * d - a * e) / (c * e * g - b * f * g - c * d * h + 75 | a * f * h + b * d * i - a * e * i)}; 76 | return MatrixNxNBase(tmp); 77 | } 78 | }; 79 | 80 | inline Matrix3x3Base operator*(const float& lhs, 81 | const Matrix3x3Base& rhs) { 82 | return Matrix3x3Base(rhs.Scale(lhs)); 83 | } 84 | 85 | inline Matrix3x3Base operator*(const double& lhs, 86 | const Matrix3x3Base& rhs) { 87 | return Matrix3x3Base(rhs.Scale(lhs)); 88 | } 89 | 90 | typedef Matrix3x3Base Matrix3x3f; 91 | typedef Matrix3x3Base Mat33f; 92 | 93 | typedef Matrix3x3Base Matrix3x3d; 94 | typedef Matrix3x3Base Mat33d; 95 | 96 | typedef Matrix3x3Base Matrix3x3; 97 | typedef Matrix3x3Base Mat33; 98 | 99 | } 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Matrix4x4.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_MATRIX4X4_H 2 | #define GEOMETRY_UTILS_MATRIX4X4_H 3 | 4 | #include "MatrixNxNBase.h" 5 | 6 | namespace geometry_utils { 7 | 8 | template 9 | struct Matrix4x4Base : MatrixNxNBase { 10 | Matrix4x4Base() : MatrixNxNBase() {} 11 | Matrix4x4Base(T val) : MatrixNxNBase(val) {} 12 | Matrix4x4Base(const Matrix4x4Base& in) : MatrixNxNBase(in.data) {} 13 | Matrix4x4Base(const boost::array& in) : MatrixNxNBase(in) {} 14 | Matrix4x4Base(T (&in)[16]) : MatrixNxNBase(in) {} 15 | Matrix4x4Base(const Eigen::Matrix& in) : MatrixNxNBase(in) {} 16 | Matrix4x4Base(const MatrixNxNBase& in) : MatrixNxNBase(in) {} 17 | Matrix4x4Base(const MatrixNxMBase& in) : MatrixNxNBase(in) {} 18 | 19 | Matrix4x4Base(T R11, T R12, T R13, T R14, T R21, T R22, T R23, T R24, T R31, 20 | T R32, T R33, T R34, T R41, T R42, T R43, T R44) { 21 | this->data[0] = R11; 22 | this->data[1] = R12; 23 | this->data[2] = R13; 24 | this->data[3] = R14; 25 | this->data[4] = R21; 26 | this->data[5] = R22; 27 | this->data[6] = R23; 28 | this->data[7] = R24; 29 | this->data[8] = R31; 30 | this->data[9] = R32; 31 | this->data[10] = R33; 32 | this->data[11] = R34; 33 | this->data[12] = R41; 34 | this->data[13] = R42; 35 | this->data[14] = R43; 36 | this->data[15] = R44; 37 | } 38 | 39 | inline T Det() const { 40 | T a = this->data[0]; 41 | T b = this->data[1]; 42 | T c = this->data[2]; 43 | T d = this->data[3]; 44 | T e = this->data[4]; 45 | T f = this->data[5]; 46 | T g = this->data[6]; 47 | T h = this->data[7]; 48 | T i = this->data[8]; 49 | T j = this->data[9]; 50 | T k = this->data[10]; 51 | T l = this->data[11]; 52 | T m = this->data[12]; 53 | T n = this->data[13]; 54 | T o = this->data[14]; 55 | T p = this->data[15]; 56 | 57 | return d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 58 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 59 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 60 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 61 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 62 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p; 63 | } 64 | 65 | virtual inline MatrixNxNBase Inv() const { 66 | if (math::fabs(Det()) < std::numeric_limits::denorm_min()) 67 | throw std::runtime_error("Matrix4x4Base: appears singular"); 68 | 69 | T a = this->data[0]; 70 | T b = this->data[1]; 71 | T c = this->data[2]; 72 | T d = this->data[3]; 73 | T e = this->data[4]; 74 | T f = this->data[5]; 75 | T g = this->data[6]; 76 | T h = this->data[7]; 77 | T i = this->data[8]; 78 | T j = this->data[9]; 79 | T k = this->data[10]; 80 | T l = this->data[11]; 81 | T m = this->data[12]; 82 | T n = this->data[13]; 83 | T o = this->data[14]; 84 | T p = this->data[15]; 85 | 86 | T tmp[16] = { 87 | (-(h * k * n) + g * l * n + h * j * o - f * l * o - g * j * p + 88 | f * k * p) / 89 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 90 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 91 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 92 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 93 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 94 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 95 | (d * k * n - c * l * n - d * j * o + b * l * o + c * j * p - 96 | b * k * p) / 97 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 98 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 99 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 100 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 101 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 102 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 103 | (-(d * g * n) + c * h * n + d * f * o - b * h * o - c * f * p + 104 | b * g * p) / 105 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 106 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 107 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 108 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 109 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 110 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 111 | (d * g * j - c * h * j - d * f * k + b * h * k + c * f * l - 112 | b * g * l) / 113 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 114 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 115 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 116 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 117 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 118 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 119 | (h * k * m - g * l * m - h * i * o + e * l * o + g * i * p - 120 | e * k * p) / 121 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 122 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 123 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 124 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 125 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 126 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 127 | (-(d * k * m) + c * l * m + d * i * o - a * l * o - c * i * p + 128 | a * k * p) / 129 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 130 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 131 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 132 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 133 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 134 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 135 | (d * g * m - c * h * m - d * e * o + a * h * o + c * e * p - 136 | a * g * p) / 137 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 138 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 139 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 140 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 141 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 142 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 143 | (-(d * g * i) + c * h * i + d * e * k - a * h * k - c * e * l + 144 | a * g * l) / 145 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 146 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 147 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 148 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 149 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 150 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 151 | (-(h * j * m) + f * l * m + h * i * n - e * l * n - f * i * p + 152 | e * j * p) / 153 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 154 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 155 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 156 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 157 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 158 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 159 | (d * j * m - b * l * m - d * i * n + a * l * n + b * i * p - 160 | a * j * p) / 161 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 162 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 163 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 164 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 165 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 166 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 167 | (-(d * f * m) + b * h * m + d * e * n - a * h * n - b * e * p + 168 | a * f * p) / 169 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 170 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 171 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 172 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 173 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 174 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 175 | (d * f * i - b * h * i - d * e * j + a * h * j + b * e * l - 176 | a * f * l) / 177 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 178 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 179 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 180 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 181 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 182 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 183 | (g * j * m - f * k * m - g * i * n + e * k * n + f * i * o - 184 | e * j * o) / 185 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 186 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 187 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 188 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 189 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 190 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 191 | (-(c * j * m) + b * k * m + c * i * n - a * k * n - b * i * o + 192 | a * j * o) / 193 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 194 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 195 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 196 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 197 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 198 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 199 | (c * f * m - b * g * m - c * e * n + a * g * n + b * e * o - 200 | a * f * o) / 201 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 202 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 203 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 204 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 205 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 206 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p), 207 | (-(c * f * i) + b * g * i + c * e * j - a * g * j - b * e * k + 208 | a * f * k) / 209 | (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + 210 | c * f * l * m - b * g * l * m - d * g * i * n + c * h * i * n + 211 | d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n + 212 | d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + 213 | b * e * l * o - a * f * l * o - c * f * i * p + b * g * i * p + 214 | c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p)}; 215 | return MatrixNxNBase(tmp); 216 | } 217 | }; 218 | 219 | inline Matrix4x4Base operator*(const float& lhs, 220 | const Matrix4x4Base& rhs) { 221 | return Matrix4x4Base(rhs.Scale(lhs)); 222 | } 223 | 224 | inline Matrix4x4Base operator*(const double& lhs, 225 | const Matrix4x4Base& rhs) { 226 | return Matrix4x4Base(rhs.Scale(lhs)); 227 | } 228 | 229 | typedef Matrix4x4Base Matrix4x4f; 230 | typedef Matrix4x4Base Mat44f; 231 | 232 | typedef Matrix4x4Base Matrix4x4d; 233 | typedef Matrix4x4Base Mat44d; 234 | 235 | typedef Matrix4x4Base Matrix4x4; 236 | typedef Matrix4x4Base Mat44; 237 | 238 | } 239 | 240 | #endif 241 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/MatrixNxMBase.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_MATRIXNXM_H 2 | #define GEOMETRY_UTILS_MATRIXNXM_H 3 | 4 | #include 5 | #include "VectorNBase.h" 6 | #include "GeometryUtilsMath.h" 7 | 8 | namespace geometry_utils { 9 | 10 | template 11 | struct MatrixNxMBase { 12 | typedef typename boost::shared_ptr > Ptr; 13 | typedef typename boost::shared_ptr > ConstPtr; 14 | 15 | static const size_t size = N * M; 16 | static const size_t nrows = N; 17 | static const size_t ncols = M; 18 | 19 | boost::array data; 20 | 21 | MatrixNxMBase() { data.fill(0); } 22 | 23 | MatrixNxMBase(T val) { data.fill(val); } 24 | 25 | MatrixNxMBase(const MatrixNxMBase& in) : data(in.data) {} 26 | 27 | MatrixNxMBase(const boost::array& in) : data(in) {} 28 | 29 | MatrixNxMBase(T (&in)[size]) { 30 | for (unsigned int i = 0; i < size; i++) 31 | data[i] = in[i]; 32 | } 33 | 34 | MatrixNxMBase(const Eigen::Matrix& in) { 35 | for (size_t i = 0; i < nrows; i++) 36 | for (size_t j = 0; j < ncols; j++) 37 | data[ncols * i + j] = in(i, j); 38 | } 39 | 40 | inline T& operator()(unsigned int i) { 41 | return data[i]; 42 | } 43 | 44 | inline const T& operator()(unsigned int i) const { 45 | return data[i]; 46 | } 47 | 48 | inline T& Get(unsigned int i) { 49 | return data[i]; 50 | } 51 | 52 | inline const T& Get(unsigned int i) const { 53 | return data[i]; 54 | } 55 | 56 | inline T& operator()(unsigned int i, unsigned int j) { 57 | return data[ncols * i + j]; 58 | } 59 | 60 | inline const T& operator()(unsigned int i, unsigned int j) const { 61 | return data[ncols * i + j]; 62 | } 63 | 64 | inline T& Get(unsigned int i, unsigned int j) { 65 | return data[ncols * i + j]; 66 | } 67 | 68 | inline const T& Get(unsigned int i, unsigned int j) const { 69 | return data[ncols * i + j]; 70 | } 71 | 72 | inline std::ostream& operator<<(std::ostream& out) { 73 | out << data; 74 | return out; 75 | } 76 | 77 | inline MatrixNxMBase& operator=(const MatrixNxMBase& rhs) { 78 | if (this == &rhs) return *this; 79 | data = rhs.data; 80 | return *this; 81 | } 82 | 83 | inline MatrixNxMBase operator*(T rhs) const { 84 | T d[size]; 85 | for (size_t i = 0; i < size; i++) 86 | d[i] = data[i] * rhs; 87 | return MatrixNxMBase(d); 88 | } 89 | 90 | inline MatrixNxMBase operator+(const MatrixNxMBase& rhs) const { 91 | T d[size]; 92 | for (size_t i = 0; i < size; i++) 93 | d[i] = data[i] + rhs(i); 94 | return MatrixNxMBase(d); 95 | } 96 | 97 | inline MatrixNxMBase operator-() const { 98 | T d[size]; 99 | for (size_t i = 0; i < size; i++) 100 | d[i] = -data[i]; 101 | return MatrixNxMBase(d); 102 | } 103 | 104 | inline MatrixNxMBase operator-(const MatrixNxMBase& rhs) const { 105 | T d[size]; 106 | for (size_t i = 0; i < size; i++) 107 | d[i] = data[i] - rhs(i); 108 | return MatrixNxMBase(d); 109 | } 110 | 111 | inline MatrixNxMBase operator*(const MatrixNxMBase& rhs) 112 | const { 113 | T d[N * N]; 114 | for (size_t i = 0; i < N; i++) 115 | for (size_t j = 0; j < N; j++) 116 | d[N * i + j] = this->Row(i) ^ rhs.Col(j); 117 | return MatrixNxMBase(d); 118 | } 119 | 120 | inline VectorNBase operator*(const VectorNBase& rhs) const { 121 | T d[nrows]; 122 | for (size_t i = 0; i < nrows; i++) 123 | d[i] = this->Row(i) ^ rhs; 124 | return VectorNBase(d); 125 | } 126 | 127 | inline MatrixNxMBase operator%(const MatrixNxMBase& rhs) const { 128 | T d[size]; 129 | for (size_t i = 0; i < size; i++) 130 | d[i] = data[i] * rhs(i); 131 | return MatrixNxMBase(d); 132 | } 133 | 134 | inline MatrixNxMBase operator/(const MatrixNxMBase& rhs) const { 135 | T d[size]; 136 | for (size_t i = 0; i < size; i++) 137 | d[i] = data[i] / rhs(i); 138 | return MatrixNxMBase(d); 139 | } 140 | 141 | inline MatrixNxMBase operator+=(const MatrixNxMBase& rhs) { 142 | for (size_t i = 0; i < size; i++) 143 | data[i] += rhs.data[i]; 144 | return *this; 145 | } 146 | 147 | inline MatrixNxMBase operator-=(const MatrixNxMBase& rhs) { 148 | for (size_t i = 0; i < size; i++) 149 | data[i] -= rhs.data[i]; 150 | return *this; 151 | } 152 | 153 | inline MatrixNxMBase operator%=(const MatrixNxMBase& rhs) { 154 | for (size_t i = 0; i < size; i++) 155 | data[i] *= rhs.data[i]; 156 | return *this; 157 | } 158 | 159 | inline MatrixNxMBase operator/=(const MatrixNxMBase& rhs) { 160 | for (size_t i = 0; i < size; i++) 161 | data[i] /= rhs.data[i]; 162 | return *this; 163 | } 164 | 165 | inline MatrixNxMBase Trans() const { 166 | T d[size]; 167 | for (size_t i = 0; i < nrows; i++) 168 | for (size_t j = 0; j < ncols; j++) 169 | d[nrows * j + i] = data[ncols * i + j]; 170 | return MatrixNxMBase(d); 171 | } 172 | 173 | inline MatrixNxMBase t() const { 174 | return this->Trans(); 175 | } 176 | 177 | inline VectorNBase Row(unsigned int r) const { 178 | T d[ncols]; 179 | for (size_t i = 0; i < ncols; i++) 180 | d[i] = data[ncols * r + i]; 181 | return VectorNBase(d); 182 | } 183 | 184 | inline VectorNBase Col(unsigned int c) const { 185 | T d[nrows]; 186 | for (size_t i = 0; i < nrows; i++) 187 | d[i] = data[ncols * i + c]; 188 | return VectorNBase(d); 189 | } 190 | 191 | inline void Ones() { data.fill(1); } 192 | 193 | inline void Zeros() { data.fill(0); } 194 | 195 | inline MatrixNxMBase Scale(T s) const { 196 | return (*this) * s; 197 | } 198 | 199 | inline void Print(const std::string& prefix = std::string()) const { 200 | if (!prefix.empty()) 201 | std::cout << prefix << std::endl; 202 | std::cout << (*this) << std::endl; 203 | } 204 | 205 | inline Eigen::Matrix Eigen() const { 206 | return Eigen::Matrix(data.data()).transpose(); 207 | } 208 | 209 | inline bool operator==(const MatrixNxMBase& that) const { 210 | return this->Equals(that); 211 | } 212 | 213 | inline bool operator!=(const MatrixNxMBase& that) const { 214 | return !this->Equals(that); 215 | } 216 | 217 | virtual inline bool Equals(const MatrixNxMBase& that, 218 | const T ptol = 1e-8) const { 219 | return (*this - that).Norm() < ptol; 220 | } 221 | 222 | inline T Norm() const { 223 | return math::sqrt((this->Trans() * (*this)).Trace()); 224 | } 225 | 226 | inline T Trace() { 227 | size_t count = nrows <= ncols ? nrows : ncols; 228 | T tr = 0; 229 | for (size_t i = 0; i < count; i++) 230 | tr += data[ncols * i + i]; 231 | return tr; 232 | } 233 | 234 | }; 235 | 236 | template 237 | inline MatrixNxMBase operator*( 238 | const float& lhs, const MatrixNxMBase& rhs) { 239 | return rhs * lhs; 240 | } 241 | 242 | template 243 | inline MatrixNxMBase operator*( 244 | const double& lhs, const MatrixNxMBase& rhs) { 245 | return rhs * lhs; 246 | } 247 | 248 | template 249 | inline std::ostream& operator<<(std::ostream& out, 250 | const MatrixNxMBase& m) { 251 | for (size_t i = 0; i < N; i++) { 252 | for (size_t j = 0; j < M; j++) 253 | out << m.data[M * i + j] << " "; 254 | out << std::endl; 255 | } 256 | return out; 257 | } 258 | 259 | template 260 | inline Eigen::Matrix Eigen(const MatrixNxMBase& in) { 261 | return in.Eigen(); 262 | } 263 | 264 | template 265 | inline MatrixNxMBase Trans(const MatrixNxMBase& m) { 266 | return m.Trans(); 267 | } 268 | 269 | template 270 | inline MatrixNxMBase Outer(const VectorNBase& v1, 271 | const VectorNBase& v2) { 272 | T d[N * M]; 273 | for (size_t i = 0; i < N; i++) 274 | for (size_t j = 0; j < M; j++) 275 | d[M * i + j] = v1(i) * v2(j); 276 | return MatrixNxMBase(d); 277 | } 278 | 279 | } 280 | 281 | #endif 282 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/MatrixNxNBase.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_MATRIXNXN_H 2 | #define GEOMETRY_UTILS_MATRIXNXN_H 3 | 4 | #include 5 | #include "VectorNBase.h" 6 | #include "GeometryUtilsMath.h" 7 | #include "MatrixNxMBase.h" 8 | 9 | namespace geometry_utils { 10 | 11 | template 12 | struct MatrixNxNBase : MatrixNxMBase { 13 | MatrixNxNBase() : MatrixNxMBase() {} 14 | MatrixNxNBase(T val) : MatrixNxMBase(val) {} 15 | MatrixNxNBase(const MatrixNxNBase& in) : MatrixNxMBase(in.data) {} 16 | MatrixNxNBase(const boost::array& in) : MatrixNxMBase(in) {} 17 | MatrixNxNBase(T (&in)[N * N]) : MatrixNxMBase(in) {} 18 | MatrixNxNBase(const Eigen::Matrix& in) 19 | : MatrixNxMBase(in) {} 20 | MatrixNxNBase(const MatrixNxMBase& in) 21 | : MatrixNxMBase(in) {} 22 | 23 | inline void Eye() { 24 | this->data.fill(0); 25 | for (size_t i = 0; i < this->nrows; i++) 26 | this->data[this->nrows * i + i] = 1; 27 | } 28 | 29 | virtual inline T Det() const { 30 | std::cerr << "MatrixNxMBase::det not implemented" << std::endl; 31 | return T(); 32 | } 33 | 34 | virtual inline MatrixNxNBase Inv() const { 35 | std::cerr << "MatrixNxMBase::inv not implemented" << std::endl; 36 | return MatrixNxNBase(); 37 | } 38 | 39 | static inline MatrixNxNBase Diag(const VectorNBase& in) { 40 | T d[N * N] = {0}; 41 | for (size_t i = 0; i < N; i++) 42 | d[N * i + i] = in(i); 43 | return MatrixNxNBase(d); 44 | } 45 | }; 46 | 47 | template 48 | inline MatrixNxNBase operator*(const float& lhs, 49 | const MatrixNxNBase& rhs) { 50 | return MatrixNxNBase(rhs * lhs); 51 | } 52 | 53 | template 54 | inline MatrixNxNBase operator*(const double& lhs, 55 | const MatrixNxNBase& rhs) { 56 | return MatrixNxNBase(rhs * lhs); 57 | } 58 | 59 | template 60 | inline MatrixNxNBase Inv(const MatrixNxNBase& m) { 61 | return m.Inv(); 62 | } 63 | 64 | } 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Quaternion.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_QUATERNION_H 2 | #define GEOMETRY_UTILS_QUATERNION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "GeometryUtilsMath.h" 8 | 9 | namespace geometry_utils { 10 | 11 | template 12 | struct QuaternionBase : VectorNBase { 13 | QuaternionBase() : VectorNBase() { 14 | this->data.assign(0); 15 | this->data[0] = 1; 16 | } 17 | 18 | QuaternionBase(T val) : VectorNBase(val) {} 19 | QuaternionBase(const QuaternionBase& in) : VectorNBase(in.data) {} 20 | QuaternionBase(const boost::array& in) : VectorNBase(in) {} 21 | QuaternionBase(T (&in)[4]) : VectorNBase(in) {} 22 | QuaternionBase(const Eigen::Quaternion& in) { 23 | this->data[0] = in.w(); 24 | this->data[1] = in.x(); 25 | this->data[2] = in.y(); 26 | this->data[3] = in.z(); 27 | } 28 | QuaternionBase(const VectorNBase& in) : VectorNBase(in) {} 29 | 30 | QuaternionBase(T w, T x, T y, T z) { 31 | this->data[0] = w; 32 | this->data[1] = x; 33 | this->data[2] = y; 34 | this->data[3] = z; 35 | } 36 | 37 | inline QuaternionBase operator*(const QuaternionBase& rhs) const { 38 | T a1 = this->data[0]; 39 | T b1 = this->data[1]; 40 | T c1 = this->data[2]; 41 | T d1 = this->data[3]; 42 | T a2 = rhs.data[0]; 43 | T b2 = rhs.data[1]; 44 | T c2 = rhs.data[2]; 45 | T d2 = rhs.data[3]; 46 | return QuaternionBase(a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2, 47 | a2 * b1 + a1 * b2 - c2 * d1 + c1 * d2, 48 | a2 * c1 + a1 * c2 + b2 * d1 - b1 * d2, 49 | -(b2 * c1) + b1 * c2 + a2 * d1 + a1 * d2); 50 | } 51 | 52 | inline T& W() { return this->data[0]; } 53 | inline const T& W() const { return this->data[0]; } 54 | 55 | inline T& X() { return this->data[1]; } 56 | inline const T& X() const { return this->data[1]; } 57 | 58 | inline T& Y() { return this->data[2]; } 59 | inline const T& Y() const { return this->data[2]; } 60 | 61 | inline T& Z() { return this->data[3]; } 62 | inline const T& Z() const { return this->data[3]; } 63 | 64 | inline QuaternionBase Conj() const { 65 | return QuaternionBase(this->data[0], -this->data[1], -this->data[2], 66 | -this->data[3]); 67 | } 68 | 69 | inline QuaternionBase Error(const QuaternionBase& q) const { 70 | return q * (*this).Conj(); 71 | } 72 | 73 | inline QuaternionBase AxisAngle() const { 74 | QuaternionBase q((*this)); 75 | if (q.W() > 1) 76 | q = q.Normalize(); 77 | T den = math::sqrt(1 - q.W() * q.W()); 78 | if (den < 1e-6) 79 | den = 1; 80 | return QuaternionBase(2 * math::acos(q.W()), q.X() / den, q.Y() / den, 81 | q.Z() / den); 82 | } 83 | }; 84 | 85 | typedef QuaternionBase Quaternionf; 86 | typedef QuaternionBase Quatf; 87 | 88 | typedef QuaternionBase Quaterniond; 89 | typedef QuaternionBase Quatd; 90 | 91 | typedef QuaternionBase Quaternion; 92 | typedef QuaternionBase Quat; 93 | 94 | } 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Rotation2.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_ROTATION2_H 2 | #define GEOMETRY_UTILS_ROTATION2_H 3 | 4 | #include 5 | #include "GeometryUtilsMath.h" 6 | #include "RotationNBase.h" 7 | #include "Matrix2x2.h" 8 | 9 | namespace geometry_utils { 10 | 11 | template 12 | struct Rotation2Base : RotationNBase { 13 | Rotation2Base() : RotationNBase() {} 14 | Rotation2Base(const Rotation2Base& in) : RotationNBase(in.data) {} 15 | Rotation2Base(const boost::array& in) : RotationNBase(in) {} 16 | Rotation2Base(T (&in)[2 * 2]) : RotationNBase(in) {} 17 | Rotation2Base(const Eigen::Matrix& in) : RotationNBase(in) {} 18 | Rotation2Base(const Eigen::Rotation2D& in) 19 | : RotationNBase(in.toRotationMatrix()) {} 20 | Rotation2Base(const RotationNBase& in) : RotationNBase(in) {} 21 | Rotation2Base(const Matrix2x2Base& in) : RotationNBase(in) {} 22 | Rotation2Base(const MatrixNxMBase& in) : RotationNBase(in) {} 23 | 24 | Rotation2Base(T val) { FromAngle(val); } 25 | 26 | Rotation2Base(T R11, T R12, T R21, T R22) { 27 | this->data[0] = R11; 28 | this->data[1] = R12; 29 | this->data[2] = R21; 30 | this->data[3] = R22; 31 | } 32 | 33 | virtual inline bool Equals(const Rotation2Base& that, 34 | const T ptol = 1e-8) const { 35 | return Error(that) < ptol; 36 | } 37 | 38 | inline T Error(const Rotation2Base& r) const { 39 | return math::sin(Angle() - r.Angle()); 40 | } 41 | 42 | inline T Angle() const { 43 | return math::atan2(this->data[2], this->data[0]); 44 | } 45 | 46 | inline void FromAngle(T val) { 47 | this->data[0] = math::cos(val); 48 | this->data[1] = -math::sin(val); 49 | this->data[2] = math::sin(val); 50 | this->data[3] = math::cos(val); 51 | } 52 | 53 | inline Eigen::Rotation2D Eigen() { 54 | return Eigen::Rotation2D(Angle()); 55 | } 56 | }; 57 | 58 | inline Rotation2Base operator*(const float& lhs, 59 | const Rotation2Base& rhs) { 60 | return Rotation2Base(rhs.Scale(lhs)); 61 | } 62 | 63 | inline Rotation2Base operator*(const double& lhs, 64 | const Rotation2Base& rhs) { 65 | return Rotation2Base(rhs.Scale(lhs)); 66 | } 67 | 68 | template 69 | inline Eigen::Rotation2D Eigen(const Rotation2Base& in) { 70 | return in.Eigen(); 71 | } 72 | 73 | typedef Rotation2Base Rotation2f; 74 | typedef Rotation2Base Rot2f; 75 | 76 | typedef Rotation2Base Rotation2d; 77 | typedef Rotation2Base Rot2d; 78 | 79 | typedef Rotation2Base Rotation2; 80 | typedef Rotation2Base Rot2; 81 | 82 | } 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Rotation3.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_ROTATION3_H 2 | #define GEOMETRY_UTILS_ROTATION3_H 3 | 4 | #include 5 | #include "GeometryUtilsMath.h" 6 | #include "RotationNBase.h" 7 | #include "Vector3.h" 8 | #include "Quaternion.h" 9 | #include "Rotation2.h" 10 | #include "Matrix2x2.h" 11 | 12 | namespace geometry_utils { 13 | 14 | template 15 | struct Rotation3Base : RotationNBase { 16 | Rotation3Base() : RotationNBase() {} 17 | Rotation3Base(const Rotation3Base& in) : RotationNBase(in.data) {} 18 | Rotation3Base(const boost::array& in) : RotationNBase(in) {} 19 | Rotation3Base(T (&in)[9]) : RotationNBase(in) {} 20 | Rotation3Base(const Eigen::Matrix& in) : RotationNBase(in) {} 21 | Rotation3Base(const Eigen::AngleAxis& in) 22 | : RotationNBase(in.toRotationMatrix()) {} 23 | Rotation3Base(const RotationNBase& in) : RotationNBase(in) {} 24 | Rotation3Base(const Matrix2x2Base& in) : RotationNBase(in) {} 25 | Rotation3Base(const MatrixNxMBase& in) : RotationNBase(in) {} 26 | 27 | Rotation3Base(T R11, T R12, T R13, T R21, T R22, T R23, T R31, T R32, T R33) { 28 | this->data[0] = R11; 29 | this->data[1] = R12; 30 | this->data[2] = R13; 31 | this->data[3] = R21; 32 | this->data[4] = R22; 33 | this->data[5] = R23; 34 | this->data[6] = R31; 35 | this->data[7] = R32; 36 | this->data[8] = R33; 37 | } 38 | 39 | Rotation3Base(const Vector3Base& in) { 40 | FromEulerZYX(in(0), in(1), in(2)); 41 | } 42 | 43 | Rotation3Base(T euler_x, T euler_y, T euler_z) { 44 | FromEulerZYX(euler_x, euler_y, euler_z); 45 | } 46 | 47 | Rotation3Base(const Rotation2Base& rot) { 48 | this->Zeros(); 49 | this->data[0] = rot(0); 50 | this->data[1] = rot(1); 51 | this->data[3] = rot(2); 52 | this->data[4] = rot(3); 53 | this->data[8] = static_cast(1); 54 | } 55 | 56 | Rotation3Base(const QuaternionBase& quat) { 57 | QuaternionBase q(quat); 58 | if (math::fabs(1 - q.Norm()) > static_cast(1e-6)) 59 | q = q.Normalize(); 60 | 61 | T a = q.W(); 62 | T b = q.X(); 63 | T c = q.Y(); 64 | T d = q.Z(); 65 | 66 | T a2 = a * a; 67 | T b2 = b * b; 68 | T c2 = c * c; 69 | T d2 = d * d; 70 | 71 | T ab = a * b; 72 | T ac = a * c; 73 | T ad = a * d; 74 | 75 | T bc = b * c; 76 | T bd = b * d; 77 | 78 | T cd = c * d; 79 | 80 | (*this)(0, 0) = a2 + b2 - c2 - d2; 81 | (*this)(0, 1) = static_cast(2) * (bc - ad); 82 | (*this)(0, 2) = static_cast(2) * (bd + ac); 83 | (*this)(1, 0) = static_cast(2) * (bc + ad); 84 | (*this)(1, 1) = a2 - b2 + c2 - d2; 85 | (*this)(1, 2) = static_cast(2) * (cd - ab); 86 | (*this)(2, 0) = static_cast(2) * (bd - ac); 87 | (*this)(2, 1) = static_cast(2) * (cd + ab); 88 | (*this)(2, 2) = a2 - b2 - c2 + d2; 89 | } 90 | 91 | virtual inline bool Equals(const Rotation3Base& that, 92 | const T ptol = 1e-8) const { 93 | return Error(that) < ptol; 94 | } 95 | 96 | inline T Error(const Rotation3Base& r) const { 97 | return Norm( 98 | static_cast(0.5) * 99 | Rotation3Base(this->Trans() * r - r.Trans() * (*this)).Vee()); 100 | } 101 | 102 | inline Vector3Base Vee() const { 103 | return Vector3Base((*this)(2, 1), (*this)(0, 2), (*this)(1, 0)); 104 | } 105 | 106 | inline void FromEulerZYX(T euler_x, T euler_y, T euler_z) { 107 | T cph = math::cos(euler_x); 108 | T sph = math::sin(euler_x); 109 | 110 | T ct = math::cos(euler_y); 111 | T st = math::sin(euler_y); 112 | 113 | T cps = math::cos(euler_z); 114 | T sps = math::sin(euler_z); 115 | 116 | (*this)(0, 0) = ct * cps; 117 | (*this)(0, 1) = cps * st * sph - cph * sps; 118 | (*this)(0, 2) = cph * cps * st + sph * sps; 119 | 120 | (*this)(1, 0) = ct * sps; 121 | (*this)(1, 1) = cph * cps + st * sph * sps; 122 | (*this)(1, 2) = -cps * sph + cph * st * sps; 123 | 124 | (*this)(2, 0) = -st; 125 | (*this)(2, 1) = ct * sph; 126 | (*this)(2, 2) = ct * cph; 127 | } 128 | 129 | inline Vector3Base GetEulerZYX() const { 130 | return ToEulerZYX(); 131 | } 132 | 133 | inline Vector3Base ToEulerZYX() const { 134 | T theta = -math::asin((*this)(2, 0)); 135 | T phi = 0; 136 | T psi = 0; 137 | if (math::fabs(cos(theta)) > static_cast(1e-6)) { 138 | phi = math::atan2((*this)(2, 1), (*this)(2, 2)); 139 | psi = math::atan2((*this)(1, 0), (*this)(0, 0)); 140 | } 141 | 142 | return Vector3Base(phi, theta, psi); 143 | } 144 | 145 | inline T Roll() const { 146 | T theta = -math::asin((*this)(2, 0)); 147 | return math::fabs(cos(theta)) > static_cast(1e-6) 148 | ? math::atan2((*this)(2, 1), (*this)(2, 2)) 149 | : 0; 150 | } 151 | 152 | inline T Pitch() const { 153 | return -math::asin((*this)(2, 0)); 154 | } 155 | 156 | inline T Yaw() const { 157 | T theta = -math::asin((*this)(2, 0)); 158 | return math::fabs(cos(theta)) > static_cast(1e-6) 159 | ? math::atan2((*this)(1, 0), (*this)(0, 0)) 160 | : 0; 161 | } 162 | }; 163 | 164 | inline Rotation3Base operator*(const float& lhs, 165 | const Rotation3Base& rhs) { 166 | return Rotation3Base(rhs.Scale(lhs)); 167 | } 168 | 169 | inline Rotation3Base operator*(const double& lhs, 170 | const Rotation3Base& rhs) { 171 | return Rotation3Base(rhs.Scale(lhs)); 172 | } 173 | 174 | template 175 | inline Rotation3Base Hat(const Vector3Base& v) { 176 | return Rotation3Base(0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0); 177 | } 178 | 179 | template 180 | inline Vector3Base Vee(const MatrixNxMBase& m) { 181 | return Rotation3Base(m).Vee(); 182 | } 183 | 184 | typedef Rotation3Base Rotation3f; 185 | typedef Rotation3Base Rot3f; 186 | 187 | typedef Rotation3Base Rotation3d; 188 | typedef Rotation3Base Rot3d; 189 | 190 | typedef Rotation3Base Rotation3; 191 | typedef Rotation3Base Rot3; 192 | 193 | } 194 | 195 | #endif 196 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/RotationNBase.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_ROTATIONN_H 2 | #define GEOMETRY_UTILS_ROTATIONN_H 3 | 4 | #include 5 | #include 6 | #include "VectorNBase.h" 7 | #include "MatrixNxNBase.h" 8 | 9 | namespace geometry_utils { 10 | 11 | template 12 | struct RotationNBase : MatrixNxNBase { 13 | RotationNBase() : MatrixNxNBase() { this->Eye(); } 14 | 15 | RotationNBase(const RotationNBase& in) : MatrixNxNBase(in.data) {} 16 | RotationNBase(const boost::array& in) : MatrixNxNBase(in) {} 17 | RotationNBase(T (&in)[N * N]) : MatrixNxNBase(in) {} 18 | RotationNBase(const Eigen::Matrix& in) : MatrixNxNBase(in) {} 19 | RotationNBase(const MatrixNxNBase& in) : MatrixNxNBase(in) {} 20 | 21 | virtual inline MatrixNxNBase Inv() const { 22 | return this->Trans(); 23 | } 24 | }; 25 | 26 | template 27 | inline RotationNBase operator*(const float& lhs, 28 | const RotationNBase& rhs) { 29 | return RotationNBase(rhs * lhs); 30 | } 31 | 32 | template 33 | inline RotationNBase operator*(const double& lhs, 34 | const RotationNBase& rhs) { 35 | return RotationNBase(rhs * lhs); 36 | } 37 | 38 | template 39 | inline RotationNBase Inv(const RotationNBase& m) { 40 | return m.Inv(); 41 | } 42 | 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Transform2.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_TRANSFORM2_H 2 | #define GEOMETRY_UTILS_TRANSFORM2_H 3 | 4 | #include "Vector2.h" 5 | #include "Rotation2.h" 6 | 7 | namespace geometry_utils { 8 | 9 | template 10 | struct Transform2Base { 11 | typedef boost::shared_ptr Ptr; 12 | typedef boost::shared_ptr ConstPtr; 13 | 14 | Vector2Base translation; 15 | Rotation2Base rotation; 16 | 17 | Transform2Base() { 18 | translation.Zeros(); 19 | rotation.Eye(); 20 | } 21 | 22 | Transform2Base(const Vector2Base& translation_, 23 | const Rotation2Base& rotation_) 24 | : translation(translation_), rotation(rotation_) {} 25 | 26 | Transform2Base(const Transform2Base& in) 27 | : translation(in.translation), rotation(in.rotation) {} 28 | 29 | Transform2Base(T x, T y, T th) : translation(x, y), rotation(th) {} 30 | 31 | Transform2Base& operator=(const Transform2Base& rhs) { 32 | if (this == &rhs) 33 | return *this; 34 | translation = rhs.translation; 35 | rotation = rhs.rotation; 36 | return *this; 37 | } 38 | 39 | Vector2Base operator*(const Vector2Base& p) const { 40 | return rotation * p + translation; 41 | } 42 | 43 | Transform2Base operator+(const Transform2Base& t) const { 44 | return Transform2Base(translation + rotation * t.translation, 45 | rotation * t.rotation); 46 | } 47 | 48 | bool operator==(const Transform2Base& that) const { 49 | return this->Equals(that); 50 | } 51 | 52 | bool operator!=(const Transform2Base& that) const { 53 | return !this->Equals(that); 54 | } 55 | 56 | bool Equals(const Transform2Base& that, const T ptol = 1e-5, 57 | const T rtol = 1e-5) const { 58 | return (translation.Equals(that.translation, ptol) && 59 | rotation.Equals(that.rotation, rtol)); 60 | } 61 | 62 | void Print(const std::string& prefix = std::string()) const { 63 | if (!prefix.empty()) 64 | std::cout << prefix << std::endl; 65 | std::cout << (*this) << std::endl; 66 | } 67 | 68 | static Transform2Base Identity() { 69 | return Transform2Base(); 70 | } 71 | }; 72 | 73 | template 74 | std::ostream& operator<<(std::ostream& out, const Transform2Base& m) { 75 | out << "Translation:" << std::endl << m.translation << std::endl; 76 | out << "Rotation:" << std::endl << m.rotation; 77 | return out; 78 | } 79 | 80 | template 81 | Transform2Base PoseUpdate(const Transform2Base& t1, 82 | const Transform2Base& t2) { 83 | return Transform2Base(t1.translation + t1.rotation * t2.translation, 84 | t1.rotation * t2.rotation); 85 | } 86 | 87 | template 88 | Transform2Base PoseInverse(const Transform2Base& t) { 89 | return Transform2Base(-1.0 * t.rotation.Trans() * t.translation, 90 | t.rotation.Trans()); 91 | } 92 | 93 | template 94 | Transform2Base PoseDelta(const Transform2Base& t1, 95 | const Transform2Base& t2) { 96 | return Transform2Base( 97 | t1.rotation.Trans() * (t2.translation - t1.translation), 98 | t1.rotation.Trans() * t2.rotation); 99 | } 100 | 101 | typedef Transform2Base Transform2f; 102 | typedef Transform2Base Transform2d; 103 | typedef Transform2d Transform2; 104 | typedef Transform2 Tr2; 105 | 106 | } 107 | 108 | #endif 109 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Transform3.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_TRANSFORM3_H 2 | #define GEOMETRY_UTILS_TRANSFORM3_H 3 | 4 | #include "Vector3.h" 5 | #include "Rotation3.h" 6 | #include "Transform2.h" 7 | 8 | namespace geometry_utils { 9 | 10 | template 11 | struct Transform3Base { 12 | typedef boost::shared_ptr Ptr; 13 | typedef boost::shared_ptr ConstPtr; 14 | 15 | Vector3Base translation; 16 | Rotation3Base rotation; 17 | 18 | Transform3Base() { 19 | translation.Zeros(); 20 | rotation.Eye(); 21 | } 22 | 23 | Transform3Base(const Vector3Base& translation_, 24 | const Rotation3Base& rotation_) 25 | : translation(translation_), rotation(rotation_) {} 26 | 27 | Transform3Base(const Transform3Base& in) 28 | : translation(in.translation), rotation(in.rotation) {} 29 | 30 | Transform3Base(const Transform2Base& in) { 31 | translation(0) = in.translation(0); 32 | translation(1) = in.translation(1); 33 | translation(2) = 0; 34 | rotation.Eye(); 35 | for (unsigned int i = 0; i < 2; i++) 36 | for (unsigned int j = 0; j < 2; j++) rotation(i, j) = in.Rotation(i, j); 37 | } 38 | 39 | Transform3Base& operator=(const Transform3Base& rhs) { 40 | if (this == &rhs) 41 | return *this; 42 | translation = rhs.translation; 43 | rotation = rhs.rotation; 44 | return *this; 45 | } 46 | 47 | Vector3Base operator*(const Vector3Base& p) const { 48 | return rotation * p + translation; 49 | } 50 | 51 | Transform3Base operator+(const Transform3Base& t) const { 52 | return Transform3Base(translation + rotation * t.translation, 53 | rotation * t.rotation); 54 | } 55 | 56 | bool operator==(const Transform3Base& that) const { 57 | return this->Equals(that); 58 | } 59 | 60 | bool operator!=(const Transform3Base& that) const { 61 | return !this->Equals(that); 62 | } 63 | 64 | bool Equals(const Transform3Base& that, const T ptol = 1e-5, 65 | const T rtol = 1e-5) const { 66 | return (translation.Equals(that.translation, ptol) && 67 | rotation.Equals(that.rotation, rtol)); 68 | } 69 | 70 | void Print(const std::string& prefix = std::string()) const { 71 | if (!prefix.empty()) 72 | std::cout << prefix << std::endl; 73 | std::cout << (*this) << std::endl; 74 | } 75 | 76 | static Transform3Base Identity() { 77 | return Transform3Base(); 78 | } 79 | }; 80 | 81 | template 82 | std::ostream& operator<<(std::ostream& out, const Transform3Base& m) { 83 | out << "Translation:" << std::endl << m.translation << std::endl; 84 | out << "Rotation:" << std::endl << m.rotation; 85 | return out; 86 | } 87 | 88 | template 89 | Transform3Base PoseUpdate(const Transform3Base& t1, 90 | const Transform3Base& t2) { 91 | return Transform3Base(t1.translation + t1.rotation * t2.translation, 92 | t1.rotation * t2.rotation); 93 | } 94 | 95 | template 96 | Transform3Base PoseInverse(const Transform3Base& t) { 97 | return Transform3Base(-1.0 * t.rotation.Trans() * t.translation, 98 | t.rotation.Trans()); 99 | } 100 | 101 | template 102 | Transform3Base PoseDelta(const Transform3Base& t1, 103 | const Transform3Base& t2) { 104 | return Transform3Base( 105 | t1.rotation.Trans() * (t2.translation - t1.translation), 106 | t1.rotation.Trans() * t2.rotation); 107 | } 108 | 109 | typedef Transform3Base Transform3f; 110 | typedef Transform3Base Transform3d; 111 | typedef Transform3d Transform3; 112 | typedef Transform3 Tr3; 113 | 114 | } 115 | 116 | #endif 117 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Vector2.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_VECTOR2_H 2 | #define GEOMETRY_UTILS_VECTOR2_H 3 | 4 | #include "VectorNBase.h" 5 | 6 | namespace geometry_utils { 7 | 8 | template 9 | struct Vector2Base : VectorNBase { 10 | Vector2Base() : VectorNBase() {} 11 | Vector2Base(T val) : VectorNBase(val) {} 12 | Vector2Base(const Vector2Base& in) : VectorNBase(in.data) {} 13 | Vector2Base(const boost::array& in) : VectorNBase(in) {} 14 | Vector2Base(T (&in)[2]) : VectorNBase(in) {} 15 | Vector2Base(const Eigen::Matrix& in) : VectorNBase(in) {} 16 | Vector2Base(const VectorNBase& in) : VectorNBase(in) {} 17 | 18 | Vector2Base(T v1, T v2) { 19 | this->data[0] = v1; 20 | this->data[1] = v2; 21 | } 22 | 23 | T X() const { return this->data[0]; } 24 | T Y() const { return this->data[1]; } 25 | }; 26 | 27 | inline Vector2Base operator*(const float& lhs, 28 | const Vector2Base& rhs) { 29 | return Vector2Base(rhs * lhs); 30 | } 31 | 32 | inline Vector2Base operator*(const double& lhs, 33 | const Vector2Base& rhs) { 34 | return Vector2Base(rhs * lhs); 35 | } 36 | 37 | typedef Vector2Base Vector2f; 38 | typedef Vector2Base Vec2f; 39 | 40 | typedef Vector2Base Vector2d; 41 | typedef Vector2Base Vec2d; 42 | 43 | typedef Vector2Base Vector2; 44 | typedef Vector2Base Vec2; 45 | 46 | } 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Vector3.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_VECTOR3_H 2 | #define GEOMETRY_UTILS_VECTOR3_H 3 | 4 | #include "VectorNBase.h" 5 | 6 | namespace geometry_utils { 7 | 8 | template 9 | struct Vector3Base : VectorNBase { 10 | Vector3Base() : VectorNBase() {} 11 | Vector3Base(T val) : VectorNBase(val) {} 12 | Vector3Base(const Vector3Base& in) : VectorNBase(in.data) {} 13 | Vector3Base(const boost::array& in) : VectorNBase(in) {} 14 | Vector3Base(T (&in)[3]) : VectorNBase(in) {} 15 | Vector3Base(const Eigen::Matrix& in) : VectorNBase(in) {} 16 | Vector3Base(const VectorNBase& in) : VectorNBase(in) {} 17 | 18 | Vector3Base(T v1, T v2, T v3) { 19 | this->data[0] = v1; 20 | this->data[1] = v2; 21 | this->data[2] = v3; 22 | } 23 | 24 | T X() const { return this->data[0]; } 25 | T Y() const { return this->data[1]; } 26 | T Z() const { return this->data[2]; } 27 | 28 | inline Vector3Base Cross(const Vector3Base& v) const { 29 | return Vector3Base(-(*this)(2) * v(1) + (*this)(1) * v(2), 30 | (*this)(2) * v(0) - (*this)(0) * v(2), 31 | -(*this)(1) * v(0) + (*this)(0) * v(1)); 32 | } 33 | }; 34 | 35 | inline Vector3Base operator*(const float& lhs, 36 | const Vector3Base& rhs) { 37 | return Vector3Base(rhs * lhs); 38 | } 39 | 40 | inline Vector3Base operator*(const double& lhs, 41 | const Vector3Base& rhs) { 42 | return Vector3Base(rhs * lhs); 43 | } 44 | 45 | template 46 | inline VectorNBase Cross(const VectorNBase& v1, 47 | const VectorNBase& v2) { 48 | return Vector3Base(v1).Cross(v2); 49 | } 50 | 51 | typedef Vector3Base Vector3f; 52 | typedef Vector3Base Vec3f; 53 | 54 | typedef Vector3Base Vector3d; 55 | typedef Vector3Base Vec3d; 56 | 57 | typedef Vector3Base Vector3; 58 | typedef Vector3Base Vec3; 59 | 60 | } 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/Vector4.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_VECTOR4_H 2 | #define GEOMETRY_UTILS_VECTOR4_H 3 | 4 | #include "VectorNBase.h" 5 | 6 | namespace geometry_utils { 7 | 8 | template 9 | struct Vector4Base : VectorNBase { 10 | Vector4Base() : VectorNBase() {} 11 | Vector4Base(T val) : VectorNBase(val) {} 12 | Vector4Base(const Vector4Base& in) : VectorNBase(in.data) {} 13 | Vector4Base(const boost::array& in) : VectorNBase(in) {} 14 | Vector4Base(T (&in)[4]) : VectorNBase(in) {} 15 | Vector4Base(const Eigen::Matrix& in) : VectorNBase(in) {} 16 | Vector4Base(const VectorNBase& in) : VectorNBase(in) {} 17 | 18 | Vector4Base(T v1, T v2, T v3, T v4) { 19 | this->data[0] = v1; 20 | this->data[1] = v2; 21 | this->data[2] = v3; 22 | this->data[3] = v4; 23 | } 24 | }; 25 | 26 | inline Vector4Base operator*(const float& lhs, 27 | const Vector4Base& rhs) { 28 | return Vector4Base(rhs * lhs); 29 | } 30 | 31 | inline Vector4Base operator*(const double& lhs, 32 | const Vector4Base& rhs) { 33 | return Vector4Base(rhs * lhs); 34 | } 35 | 36 | typedef Vector4Base Vector4f; 37 | typedef Vector4Base Vec4f; 38 | 39 | typedef Vector4Base Vector4d; 40 | typedef Vector4Base Vec4d; 41 | 42 | typedef Vector4Base Vector4; 43 | typedef Vector4Base Vec4; 44 | 45 | } 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/VectorNBase.h: -------------------------------------------------------------------------------- 1 | #ifndef GEOMETRY_UTILS_VECTORN_H 2 | #define GEOMETRY_UTILS_VECTORN_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "GeometryUtilsMath.h" 10 | 11 | namespace geometry_utils { 12 | 13 | template 14 | struct VectorNBase { 15 | typedef typename boost::shared_ptr > Ptr; 16 | typedef typename boost::shared_ptr > ConstPtr; 17 | 18 | static const size_t length = N; 19 | 20 | boost::array data; 21 | 22 | VectorNBase() { data.fill(0); } 23 | 24 | VectorNBase(T val) { data.fill(val); } 25 | 26 | VectorNBase(const VectorNBase& in) : data(in.data) {} 27 | 28 | VectorNBase(const boost::array& in) : data(in) {} 29 | 30 | VectorNBase(T (&in)[N]) { 31 | for (size_t i = 0; i < N; i++) 32 | data[i] = in[i]; 33 | } 34 | 35 | VectorNBase(const Eigen::Matrix& in) { 36 | for (size_t i = 0; i < N; i++) 37 | data[i] = in(i, 0); 38 | } 39 | 40 | inline T& operator()(unsigned int i) { 41 | return data[i]; 42 | } 43 | 44 | inline const T& operator()(unsigned int i) const { 45 | return data[i]; 46 | } 47 | 48 | inline T& Get(unsigned int i) { 49 | return data[i]; 50 | } 51 | 52 | inline const T& Get(unsigned int i) const { 53 | return data[i]; 54 | } 55 | 56 | inline VectorNBase& operator=(const VectorNBase& rhs) { 57 | if (this == &rhs) 58 | return *this; 59 | data = rhs.data; 60 | return *this; 61 | } 62 | 63 | inline VectorNBase operator*(T rhs) const { 64 | T d[N]; 65 | for (size_t i = 0; i < N; i++) 66 | d[i] = data[i] * rhs; 67 | return VectorNBase(d); 68 | } 69 | 70 | inline VectorNBase operator+(const VectorNBase& rhs) const { 71 | T d[N]; 72 | for (size_t i = 0; i < N; i++) 73 | d[i] = data[i] + rhs.data[i]; 74 | return VectorNBase(d); 75 | } 76 | 77 | inline VectorNBase operator-() const { 78 | T d[N]; 79 | for (size_t i = 0; i < N; i++) 80 | d[i] = -data[i]; 81 | return VectorNBase(d); 82 | } 83 | 84 | inline VectorNBase operator-(const VectorNBase& rhs) const { 85 | T d[N]; 86 | for (size_t i = 0; i < N; i++) 87 | d[i] = data[i] - rhs.data[i]; 88 | return VectorNBase(d); 89 | } 90 | 91 | inline VectorNBase operator%(const VectorNBase& rhs) const { 92 | T d[N]; 93 | for (size_t i = 0; i < N; i++) 94 | d[i] = data[i] * rhs.data[i]; 95 | return VectorNBase(d); 96 | } 97 | 98 | inline T operator^(const VectorNBase& rhs) const { 99 | T dot = 0; 100 | for (size_t i = 0; i < N; i++) 101 | dot += data[i] * rhs.data[i]; 102 | return dot; 103 | } 104 | 105 | inline VectorNBase operator/(const VectorNBase& rhs) const { 106 | T d[N]; 107 | for (size_t i = 0; i < N; i++) 108 | d[i] = data[i] / rhs.data[i]; 109 | return VectorNBase(d); 110 | } 111 | 112 | inline VectorNBase operator+=(const VectorNBase& rhs) { 113 | for (size_t i = 0; i < N; i++) 114 | data[i] += rhs.data[i]; 115 | return *this; 116 | } 117 | 118 | inline VectorNBase operator-=(const VectorNBase& rhs) { 119 | for (size_t i = 0; i < N; i++) 120 | data[i] -= rhs.data[i]; 121 | return *this; 122 | } 123 | 124 | inline VectorNBase operator%=(const VectorNBase& rhs) { 125 | for (size_t i = 0; i < N; i++) 126 | data[i] *= rhs.data[i]; 127 | return *this; 128 | } 129 | 130 | inline VectorNBase operator/=(const VectorNBase& rhs) { 131 | for (size_t i = 0; i < N; i++) 132 | data[i] /= rhs.data[i]; 133 | return *this; 134 | } 135 | 136 | inline VectorNBase operator*=(const T& rhs) { 137 | for (size_t i = 0; i < N; i++) 138 | data[i] *= rhs; 139 | return *this; 140 | } 141 | 142 | inline VectorNBase operator/=(const T& rhs) { 143 | for (size_t i = 0; i < N; i++) 144 | data[i] /= rhs; 145 | return *this; 146 | } 147 | 148 | inline bool operator==(const VectorNBase& that) const { 149 | return this->Equals(that); 150 | } 151 | 152 | inline bool operator!=(const VectorNBase& that) const { 153 | return !this->Equals(that); 154 | } 155 | 156 | inline bool Equals(const VectorNBase& that, const T ptol = 1e-8) const { 157 | return (*this - that).Norm() < ptol; 158 | } 159 | 160 | inline T Norm() const { return math::sqrt((*this) ^ (*this)); } 161 | 162 | inline VectorNBase Normalize() const { return (*this) / Norm(); } 163 | 164 | inline VectorNBase Abs() const { 165 | T d[N]; 166 | for (size_t i = 0; i < N; i++) 167 | d[i] = std::abs(data[i]); 168 | return VectorNBase(d); 169 | } 170 | 171 | inline void Ones() { 172 | data.fill(1); 173 | } 174 | 175 | inline void Zeros() { 176 | data.fill(0); 177 | } 178 | 179 | inline T Dot(const VectorNBase& v) const { 180 | return (*this) ^ v; 181 | } 182 | 183 | inline VectorNBase Scale(T s) const { 184 | return (*this) * s; 185 | } 186 | 187 | inline void Print(const std::string& prefix = std::string()) const { 188 | if (!prefix.empty()) 189 | std::cout << prefix << std::endl; 190 | std::cout << (*this) << std::endl; 191 | } 192 | 193 | inline Eigen::Matrix Eigen() const { 194 | return Eigen::Matrix(data.data()); 195 | } 196 | }; 197 | 198 | template 199 | inline VectorNBase operator*(const T& lhs, const VectorNBase& rhs) { 200 | return rhs * lhs; 201 | } 202 | 203 | template 204 | inline std::ostream& operator<<(std::ostream& out, const VectorNBase& m) { 205 | for (size_t i = 0; i < N - 1; i++) 206 | out << m.data[i] << " "; 207 | out << m.data[N - 1]; 208 | return out; 209 | } 210 | 211 | template 212 | inline Eigen::Matrix Eigen(const VectorNBase& in) { 213 | return in.Eigen(); 214 | } 215 | 216 | template 217 | inline T Norm(const VectorNBase& v) { 218 | return v.Norm(); 219 | } 220 | 221 | template 222 | inline T Dot(const VectorNBase& v1, const VectorNBase& v2) { 223 | return v1.Dot(v2); 224 | } 225 | 226 | } 227 | 228 | #endif 229 | -------------------------------------------------------------------------------- /geometry_utils/include/geometry_utils/YAMLLoader.h: -------------------------------------------------------------------------------- 1 | /* 2 | geometry_utils: Utility library to provide common geometry types and transformations 3 | Copyright (C) 2013 Nathan Michael 4 | 2016 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef GEOMETRY_UTILS_YAML_LOADER_H 22 | #define GEOMETRY_UTILS_YAML_LOADER_H 23 | 24 | #ifdef __YAML_UTILS_H__ 25 | #error "YAMLUtils must be included last" 26 | #endif 27 | 28 | #include 29 | #include 30 | 31 | namespace YAML { 32 | 33 | template <> 34 | struct convert { 35 | static Node Encode(const geometry_utils::Vector2& rhs) { 36 | Node node; 37 | node.push_back(rhs[0]); 38 | node.push_back(rhs[1]); 39 | return node; 40 | } 41 | 42 | static bool Decode(const Node& node, geometry_utils::Vector2& rhs) { 43 | if (!node.IsSequence() || node.size() != 2) 44 | return false; 45 | 46 | rhs[0] = node[0].as(); 47 | rhs[1] = node[1].as(); 48 | return true; 49 | } 50 | }; 51 | 52 | template <> 53 | struct convert { 54 | static Node Encode(const geometry_utils::Vector3& rhs) { 55 | Node node; 56 | node.push_back(rhs[0]); 57 | node.push_back(rhs[1]); 58 | node.push_back(rhs[2]); 59 | return node; 60 | } 61 | 62 | static bool Decode(const Node& node, geometry_utils::Vector3& rhs) { 63 | if (!node.IsSequence() || node.size() != 3) 64 | return false; 65 | 66 | rhs[0] = node[0].as(); 67 | rhs[1] = node[1].as(); 68 | rhs[2] = node[2].as(); 69 | return true; 70 | } 71 | }; 72 | 73 | template <> 74 | struct convert { 75 | static Node Encode(const geometry_utils::Vector4& rhs) { 76 | Node node; 77 | node.push_back(rhs[0]); 78 | node.push_back(rhs[1]); 79 | node.push_back(rhs[2]); 80 | node.push_back(rhs[3]); 81 | return node; 82 | } 83 | 84 | static bool Decode(const Node& node, geometry_utils::Vector4& rhs) { 85 | if (!node.IsSequence() || node.size() != 4) 86 | return false; 87 | 88 | rhs[0] = node[0].as(); 89 | rhs[1] = node[1].as(); 90 | rhs[2] = node[2].as(); 91 | rhs[3] = node[3].as(); 92 | return true; 93 | } 94 | }; 95 | 96 | } 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /geometry_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | geometry_utils 4 | 0.0.9 5 | geometry_utils 6 | 7 | Nathan Michael 8 | Erik Nelson 9 | Erik Nelson 10 | 11 | GPLv2 12 | 13 | catkin 14 | roscpp 15 | geometry_msgs 16 | tf2 17 | 18 | tf2 19 | roscpp 20 | 21 | -------------------------------------------------------------------------------- /geometry_utils/tests/test_equals.cc: -------------------------------------------------------------------------------- 1 | #define BOOST_TEST_DYN_LINK 2 | #define BOOST_TEST_MODULE test_equals 3 | #include 4 | #include 5 | 6 | namespace gu = geometry_utils; 7 | 8 | BOOST_AUTO_TEST_CASE(transform_equals_exact) { 9 | gu::Transform3 t1; 10 | Eigen::Vector3d p1 = Eigen::Vector3d::Random(); 11 | Eigen::Vector3d p2 = Eigen::Vector3d::Random(); 12 | t1.translation = gu::Vec3(p1); 13 | t1.rotation = gu::ZYXToR(gu::Vec3(p2)); 14 | 15 | // Check that two random transforms are 16 | // equal after copy assignment 17 | gu::Transform3 t2 = t1; 18 | 19 | BOOST_CHECK( t1.Equals(t2) ); 20 | BOOST_CHECK( t2.Equals(t1) ); 21 | 22 | // Check for equivalence after 23 | // copy construction 24 | gu::Transform3 t3(t2); 25 | BOOST_CHECK( t3.Equals(t2) ); 26 | BOOST_CHECK( t2.Equals(t3) ); 27 | BOOST_CHECK( t1.Equals(t3) ); 28 | BOOST_CHECK( t3.Equals(t1) ); 29 | } 30 | 31 | BOOST_AUTO_TEST_CASE(transform_equals_pos_tol) { 32 | gu::Transform3 t1; 33 | Eigen::Vector3d p1 = Eigen::Vector3d::Random(); 34 | Eigen::Vector3d p2 = Eigen::Vector3d::Random(); 35 | t1.translation = gu::Vec3(p1); 36 | t1.rotation = gu::ZYXToR(gu::Vec3(p2)); 37 | 38 | gu::Transform3 t2 = t1; 39 | 40 | double ptol = 1e-5; 41 | double ptol_element = sqrt(pow(ptol, 2.0) / 3.0); 42 | t2.translation(0) += ptol_element; 43 | t2.translation(1) += ptol_element; 44 | t2.translation(2) += ptol_element; 45 | 46 | // Check that they are not equal when slightly over the tolerance 47 | BOOST_CHECK( ! t1.Equals(t2, ptol - 1e-10) ); 48 | BOOST_CHECK( ! t2.Equals(t1, ptol - 1e-10) ); 49 | 50 | // Check that they are equal when slightly under the tolerance 51 | BOOST_CHECK( t1.Equals(t2, ptol + 1e-10) ); 52 | BOOST_CHECK( t2.Equals(t1, ptol + 1e-10) ); 53 | } 54 | 55 | BOOST_AUTO_TEST_CASE(transform_equals_rot_tol) { 56 | gu::Transform3 t1; 57 | Eigen::Vector3d p1 = Eigen::Vector3d::Random(); 58 | Eigen::Vector3d p2 = Eigen::Vector3d::Random(); 59 | t1.translation = gu::Vec3(p1); 60 | t1.rotation = gu::ZYXToR(gu::Vec3(p2)); 61 | 62 | gu::Transform3 t2 = t1; 63 | 64 | double ptol = 1e-5; 65 | double rtol = 1e-5; 66 | 67 | srand(10); 68 | Eigen::Vector3d p3 = Eigen::Vector3d::Random(); 69 | gu::Vector3 small_noise(1e-6*p3); 70 | 71 | t2.rotation = gu::ZYXToR(gu::RToZYX(t1.rotation) + small_noise); 72 | 73 | // Check that they equal when under tolerance 74 | BOOST_CHECK( t1.Equals(t2, ptol, rtol) ); 75 | BOOST_CHECK( t2.Equals(t1, ptol, rtol) ); 76 | 77 | gu::Vector3 big_noise = small_noise * 1e4; 78 | t2.rotation = gu::ZYXToR(gu::RToZYX(t1.rotation) + big_noise); 79 | 80 | // Check that they are unequal when slightly over tolerance 81 | BOOST_CHECK( ! t1.Equals(t2, ptol, rtol) ); 82 | BOOST_CHECK( ! t2.Equals(t1, ptol, rtol) ); 83 | 84 | } 85 | 86 | BOOST_AUTO_TEST_CASE(transform_operator_bool_eq) { 87 | gu::Transform3 t1; 88 | Eigen::Vector3d p1 = Eigen::Vector3d::Random(); 89 | Eigen::Vector3d p2 = Eigen::Vector3d::Random(); 90 | t1.translation = gu::Vec3(p1); 91 | t1.rotation = gu::ZYXToR(gu::Vec3(p2)); 92 | 93 | // Check that two random transforms are 94 | // equal after copy assignment 95 | gu::Transform3 t2 = t1; 96 | BOOST_CHECK( t1 == t2 ); 97 | BOOST_CHECK( t2 == t1 ); 98 | 99 | // Check for equivalence after 100 | // copy construction 101 | gu::Transform3 t3(t2); 102 | BOOST_CHECK( t3 == t2 ); 103 | BOOST_CHECK( t2 == t3 ); 104 | BOOST_CHECK( t1 == t3 ); 105 | BOOST_CHECK( t3 == t1 ); 106 | } 107 | 108 | BOOST_AUTO_TEST_CASE(transform_operator_bool_ineq) { 109 | gu::Transform3 t1; 110 | Eigen::Vector3d p1 = Eigen::Vector3d::Random(); 111 | t1.translation = gu::Vec3(p1); 112 | 113 | gu::Vector3 euler_noise = t1.translation; 114 | t1.rotation = gu::ZYXToR(euler_noise); 115 | 116 | // Check that two random transforms are 117 | // equal after copy assignment 118 | gu::Transform3 t2 = t1; 119 | BOOST_CHECK( ! (t1 != t2) ); 120 | BOOST_CHECK( ! (t2 != t1) ); 121 | 122 | // Check for equivalence after 123 | // copy construction 124 | gu::Transform3 t3(t2); 125 | BOOST_CHECK( ! (t3 != t2) ); 126 | BOOST_CHECK( ! (t2 != t3) ); 127 | BOOST_CHECK( ! (t1 != t3) ); 128 | BOOST_CHECK( ! (t3 != t1) ); 129 | 130 | // Make a new transform with same pos, 131 | // but different rot from t1, t2, t3 132 | gu::Transform3 t4; 133 | Eigen::Vector3d p2 = Eigen::Vector3d::Random(); 134 | t4.translation = t1.translation; 135 | euler_noise = gu::Vec3(p2); 136 | t4.rotation = gu::ZYXToR(euler_noise); 137 | 138 | // Make sure the rotation is not 139 | // randomly generated close t1's rotation 140 | gu::Vector3 rot_err = gu::Vee(Trans(t4.rotation) * t1.rotation - 141 | Trans(t1.rotation) * t4.rotation); 142 | double rtol = 1e-5; 143 | if (Norm(0.5 * rot_err) < rtol) { 144 | euler_noise(0) += M_PI / 4.0; 145 | t4.rotation = gu::ZYXToR(euler_noise); 146 | } 147 | 148 | BOOST_CHECK( t4 != t1 ); 149 | BOOST_CHECK( t1 != t4 ); 150 | 151 | // Make a new transform with same rot, 152 | // but different pos from t1, t2, t3 153 | gu::Transform3 t5; 154 | Eigen::Vector3d p3 = Eigen::Vector3d::Random(); 155 | t5.translation = gu::Vec3(p3); 156 | t5.rotation = t1.rotation; 157 | 158 | // Make sure the translation is not 159 | // randomly generated close to t1's translation 160 | double pos_err = Norm(t5.translation - t1.translation); 161 | double ptol = 1e-5; 162 | if (pos_err < ptol) 163 | t5.translation(0) += ptol*10; 164 | 165 | BOOST_CHECK( t5 != t1 ); 166 | BOOST_CHECK( t1 != t5 ); 167 | } 168 | -------------------------------------------------------------------------------- /geometry_utils/tests/test_math.cc: -------------------------------------------------------------------------------- 1 | #define BOOST_TEST_DYN_LINK 2 | #define BOOST_TEST_MODULE test_math 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace gu = geometry_utils; 9 | namespace gm = gu::math; 10 | 11 | BOOST_AUTO_TEST_CASE(math) { 12 | srand(time(NULL)); 13 | float rf = static_cast(rand())/static_cast(RAND_MAX); 14 | double rd = static_cast(rand())/static_cast(RAND_MAX); 15 | 16 | BOOST_CHECK_EQUAL(gm::cos(rf), cosf(rf)); 17 | BOOST_CHECK_EQUAL(gm::cos(rf), gm::cos(rf)); 18 | BOOST_CHECK_EQUAL(gm::cos(rd), cos(rd)); 19 | BOOST_CHECK_EQUAL(gm::cos(rd), gm::cos(rd)); 20 | 21 | BOOST_CHECK_EQUAL(gm::acos(rf), acosf(rf)); 22 | BOOST_CHECK_EQUAL(gm::acos(rf), gm::acos(rf)); 23 | BOOST_CHECK_EQUAL(gm::acos(rd), acos(rd)); 24 | BOOST_CHECK_EQUAL(gm::acos(rd), gm::acos(rd)); 25 | 26 | BOOST_CHECK_EQUAL(gm::sin(rf), sinf(rf)); 27 | BOOST_CHECK_EQUAL(gm::sin(rf), gm::sin(rf)); 28 | BOOST_CHECK_EQUAL(gm::sin(rd), sin(rd)); 29 | BOOST_CHECK_EQUAL(gm::sin(rd), gm::sin(rd)); 30 | 31 | BOOST_CHECK_EQUAL(gm::asin(rf), asinf(rf)); 32 | BOOST_CHECK_EQUAL(gm::asin(rf), gm::asin(rf)); 33 | BOOST_CHECK_EQUAL(gm::asin(rd), asin(rd)); 34 | BOOST_CHECK_EQUAL(gm::asin(rd), gm::asin(rd)); 35 | 36 | BOOST_CHECK_EQUAL(gm::tan(rf), tanf(rf)); 37 | BOOST_CHECK_EQUAL(gm::tan(rf), gm::tan(rf)); 38 | BOOST_CHECK_EQUAL(gm::tan(rd), tan(rd)); 39 | BOOST_CHECK_EQUAL(gm::tan(rd), gm::tan(rd)); 40 | 41 | BOOST_CHECK_EQUAL(gm::fabs(rf), fabsf(rf)); 42 | BOOST_CHECK_EQUAL(gm::fabs(rf), gm::fabs(rf)); 43 | BOOST_CHECK_EQUAL(gm::fabs(rd), fabs(rd)); 44 | BOOST_CHECK_EQUAL(gm::fabs(rd), gm::fabs(rd)); 45 | 46 | BOOST_CHECK_EQUAL(gm::sqrt(rf), sqrtf(rf)); 47 | BOOST_CHECK_EQUAL(gm::sqrt(rf), gm::sqrt(rf)); 48 | BOOST_CHECK_EQUAL(gm::sqrt(rd), sqrt(rd)); 49 | BOOST_CHECK_EQUAL(gm::sqrt(rd), gm::sqrt(rd)); 50 | 51 | float rf1 = static_cast(rand())/static_cast(RAND_MAX); 52 | float rf2 = static_cast(rand())/static_cast(RAND_MAX); 53 | double rd1 = static_cast(rand())/static_cast(RAND_MAX); 54 | double rd2 = static_cast(rand())/static_cast(RAND_MAX); 55 | 56 | BOOST_CHECK_EQUAL(gm::atan2(rf1, rf2), atan2f(rf1, rf2)); 57 | BOOST_CHECK_EQUAL(gm::atan2(rf1, rf2), gm::atan2(rf1, rf2)); 58 | BOOST_CHECK_EQUAL(gm::atan2(rd1, rd2), atan2(rd1, rd2)); 59 | BOOST_CHECK_EQUAL(gm::atan2(rd1, rd2), gm::atan2(rd1, rd2)); 60 | 61 | BOOST_CHECK_EQUAL(gm::hypot(rf1, rf2), hypotf(rf1, rf2)); 62 | BOOST_CHECK_EQUAL(gm::hypot(rf1, rf2), gm::hypot(rf1, rf2)); 63 | BOOST_CHECK_EQUAL(gm::hypot(rd1, rd2), hypot(rd1, rd2)); 64 | BOOST_CHECK_EQUAL(gm::hypot(rd1, rd2), gm::hypot(rd1, rd2)); 65 | } 66 | -------------------------------------------------------------------------------- /geometry_utils/tests/test_so3error.cc: -------------------------------------------------------------------------------- 1 | #define BOOST_TEST_DYN_LINK 2 | #define BOOST_TEST_MODULE test_equals 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace gu = geometry_utils; 9 | 10 | BOOST_AUTO_TEST_CASE(so3_matrix) { 11 | gu::Rot3 r1(0, 0, 0); 12 | gu::Rot3 r2(0, 0, 0); 13 | gu::Rot3 r3(M_PI/2.0, 0, 0); 14 | 15 | BOOST_CHECK_EQUAL( r1.Error(r2), 0.0 ); 16 | BOOST_CHECK_EQUAL( r1.Error(r3), 1.0 ); 17 | } 18 | 19 | BOOST_AUTO_TEST_CASE(so3_quat) { 20 | gu::Quat q1 = gu::RToQuat(gu::Rot3(0, 0, 0)); 21 | gu::Quat q2 = gu::RToQuat(gu::Rot3(0, 0, 0)); 22 | gu::Quat q3 = gu::RToQuat(gu::Rot3(M_PI/2.0, 0, 0)); 23 | 24 | gu::Rot3 r1(q1); 25 | gu::Rot3 r2(q2); 26 | gu::Rot3 r3(q3); 27 | 28 | gu::Vec3 v1 = r1.Row(2); 29 | gu::Vec3 v2 = r2.Row(2); 30 | gu::Vec3 v3 = r3.Row(2); 31 | 32 | BOOST_CHECK_EQUAL( acos(v1.Dot(v2)), 0.0 ); 33 | BOOST_CHECK_CLOSE( acos(v1.Dot(v3)), M_PI/2.0 , 1e-10); 34 | 35 | BOOST_CHECK_CLOSE( q1.Error(q2).AxisAngle()(0), 0.0, 1e-10); 36 | BOOST_CHECK_CLOSE( q1.Error(q3).AxisAngle()(0), M_PI/2, 1e-10); 37 | 38 | BOOST_CHECK_CLOSE( acos(v1.Dot(v2)), q1.Error(q2).AxisAngle()(0) , 1e-10); 39 | BOOST_CHECK_CLOSE( acos(v1.Dot(v3)), q1.Error(q3).AxisAngle()(0) , 1e-10); 40 | } 41 | -------------------------------------------------------------------------------- /parameter_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(parameter_utils) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | INCLUDE_DIRS include 8 | ) 9 | 10 | install(DIRECTORY include/${PROJECT_NAME}/ 11 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 12 | FILES_MATCHING PATTERN "*.h" 13 | ) 14 | -------------------------------------------------------------------------------- /parameter_utils/include/parameter_utils/ParameterUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | parameter_utils: Convenience utility functions for working with ROS parameters 3 | Copyright (C) 2013 Nathan Michael 4 | 2015 Erik Nelson 5 | 6 | This program is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU General Public License 8 | as published by the Free Software Foundation; either version 2 9 | of the License, or (at your option) any later version. 10 | 11 | This program is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU General Public License for more details. 15 | 16 | You should have received a copy of the GNU General Public License 17 | along with this program; if not, write to the Free Software 18 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 19 | */ 20 | 21 | #ifndef PARAMETER_UTILS_H 22 | #define PARAMETER_UTILS_H 23 | 24 | #include 25 | #include 26 | 27 | namespace ros { 28 | namespace param { 29 | inline bool get(const std::string& key, unsigned char& c) { 30 | int i; 31 | bool ret = ros::param::get(key, i); 32 | assert((i >= 0) && (i < 256)); 33 | c = (unsigned char)i; 34 | return ret; 35 | } 36 | 37 | inline bool get(const std::string& key, unsigned int& d) { 38 | int i; 39 | bool ret = ros::param::get(key, i); 40 | assert(i >= 0); 41 | d = (unsigned int)i; 42 | return ret; 43 | } 44 | 45 | inline bool get(const std::string& key, float& f) { 46 | double d; 47 | bool ret = ros::param::get(key, d); 48 | f = (float)d; 49 | return ret; 50 | } 51 | } 52 | } 53 | 54 | namespace parameter_utils { 55 | template 56 | bool Get(const std::string& s, M& p) { 57 | std::string name = ros::this_node::getName(); 58 | 59 | std::string r; 60 | if (!ros::param::search(s, r)) { 61 | ROS_ERROR("%s: Failed to search for parameter '%s'.", name.c_str(), 62 | s.c_str()); 63 | return false; 64 | } 65 | 66 | if (!ros::param::has(r)) { 67 | ROS_ERROR("%s: Missing required parameter '%s'.", name.c_str(), s.c_str()); 68 | return false; 69 | } 70 | 71 | if (!ros::param::get(r, p)) { 72 | ROS_ERROR("%s: Failed to get parameter '%s'.", name.c_str(), s.c_str()); 73 | return false; 74 | } 75 | 76 | return true; 77 | } 78 | 79 | template 80 | bool Get(const std::string& s, M& p, M def) { 81 | bool ret = true; 82 | 83 | std::string name = ros::this_node::getName(); 84 | 85 | std::string r; 86 | if (!ros::param::search(s, r)) { 87 | ROS_DEBUG("%s: Failed to search for parameter '%s', using default.", 88 | name.c_str(), s.c_str()); 89 | p = def; 90 | ret = false; 91 | } 92 | 93 | if (ret && !ros::param::has(r)) { 94 | ROS_DEBUG("%s: Missing required parameter '%s', using default.", 95 | name.c_str(), s.c_str()); 96 | p = def; 97 | ret = false; 98 | } 99 | 100 | if (ret && !ros::param::get(r, p)) { 101 | ROS_DEBUG("%s: Failed to get parameter '%s', using default.", name.c_str(), 102 | s.c_str()); 103 | p = def; 104 | ret = false; 105 | } 106 | 107 | return ret; 108 | } 109 | } 110 | #endif 111 | -------------------------------------------------------------------------------- /parameter_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | parameter_utils 4 | 0.0.0 5 | The parameter_utils package 6 | 7 | Nathan Michael 8 | Erik Nelson 9 | Erik Nelson 10 | 11 | GPLv2 12 | 13 | catkin 14 | roscpp 15 | 16 | --------------------------------------------------------------------------------