least squares use also odometry measurements

README.md

@@ -5,5 +5,33 @@ Range only SLAM
 ## Goal
 Estimating initial map from measurements and reduce error from trajectory and landmarks using least square algorithm
 
+
+## Results
+After Initial Guess:
+- landmark error: 3.081
+- pose error: 2.469
+
+After 25 iterations:
+
+- Using only range measurements:
+     - landmark error: 2.82
+     - pose error: 2.56
+
+- Using range and odometry measurements:
+     - landmark error: 1.569
+     - pose error: 1.6767
+
+Using also the odometry measurements in the least squares algorithm greatly reduce the error, but introduce some artifacts in the upper-right region of the path.
+
+### Results using range and odometry measurements
+
+Notice problems on top-right corner but good accuracy on trajectory
+![Results using range and odometry](trajectory_plot.png "Trajectory using range and odometry")
+![Results using range and odometry 2](landmark_plot_no_outlier.png "landmark using range and odometry")
+
+### Results using only range
+![Results using range](trajectory_plot_only_range.png "Trajectory using only range")
+![Results using range 2](landmark_plot_no_outlier_only_range.png "landmark using only range")
+
 ## Running
 Execute by running main.m

main.m

@@ -9,7 +9,7 @@
 addpath 'tools/algorithms'
 source 'tools/algorithms/initGuess.m'
 source 'tools/algorithms/leastSquares.m'
-source 'tools/algorithms/leastSquares2.m'
+source 'tools/algorithms/leastSquares_odom.m'
 source 'tools/algorithms/preprocessTransitions.m'
 source 'tools/algorithms/LeastSquaresUtils.m'
 source 'tools/algorithms/evaluate.m'
@@ -25,7 +25,14 @@
 [gt_landmarks, gt_poses, gt_transitions, gt_observations] = loadG2o(fullfile(data_path, dataset_gt));
 [~,poses,transitions,observations] = loadG2o(fullfile(data_path, dataset_ig));
 
+%get ground truth poses in matrix form, so it's easier to plot and legend work
+gt_matrix_poses=zeros(3, 3, length(gt_poses));
+for i=1:length(gt_poses)
+    gt_matrix_poses(:,:,i)=v2t([gt_poses(i).x gt_poses(i).y gt_poses(i).theta]);
+endfor
+
 %cardinality of the data
+printf("\n\nCardinality of the problem:\n")
 num_landmarks=length(gt_landmarks);
 num_poses=length(poses);
 num_transitions=length(transitions);
@@ -37,31 +44,37 @@
 first_pose_id=poses(1).id;
 params.first_pose_id=first_pose_id;
 
-params.kernel_threshold=2.0;
+params.kernel_threshold=1.0;
 params.num_iterations=25;
-params.damping=0.1;
-printf("parameters:\n")
+params.damping=1e-3;
+printf("\nParameters:\n")
 params
 
 %----INITIAL GUESS----
-printf("\n Initial Guess...\n")
+printf("\nInitial Guess...\n")
 [landmark_positions,robot_poses,land_observations,land_id_to_idx]=initGuess(observations,poses,transitions,params);
 init_guess_land=land_observations;
-printf("Error in landmark position using initial guess is %f\n",evaluate(gt_landmarks,land_observations,land_id_to_idx));
-
+printf("\nAfter initial guess...\n")
+printf("\tmean error in landmark position is %f\n",evaluate(gt_landmarks,land_observations,land_id_to_idx));
+printf("\tmean error in robot poses is %f\n",evaluatePoses(gt_matrix_poses,robot_poses));
 
 
 
 T=preprocessTransitions(transitions,poses);
 
 %---LEAST SQUARE OPTIMIZATION-------
-#printf("\n Least squares using %d iterations...\n",params.num_iterations)
-[XR,XL]=leastSquares2(land_observations,robot_poses,landmark_positions,poses,T,gt_landmarks,params,land_id_to_idx);
+printf("\n\nLeast squares using %d iterations...\n",params.num_iterations)
+
+%least_square only range
+%[XR,XL]=leastSquares(land_observations,robot_poses,landmark_positions,poses,gt_landmarks,params,land_id_to_idx);
+
+%least squares using range and odometry
+[XR,XL]=leastSquares_odom(land_observations,robot_poses,landmark_positions,poses,T,gt_landmarks,gt_matrix_poses,params,land_id_to_idx);
 
 #printf("error in landmark position after least square is %f\n",evaluate(gt_landmarks,XL,land_id_to_idx));
 
 %tuning parameters
-%this function was used to test if changing parameters of LS (kernel_threshold, damping) had some effect
+%this function was used to test if changing parameters of LS (e.g.  damping) had some effect
 %[XR,XL]=tuning(land_observations,robot_poses,landmark_positions,poses,gt_landmarks,params,land_id_to_idx);
 
 
@@ -72,11 +85,6 @@
 %this functions however don't use traditional octave plot so the legend of the other plot will be wrong
 %drawLandmarkWithLabel(XL,gt_landmarks,radius=0.1)
 
-%get ground truth poses in matrix form, so it's easier to plot and legend work
-gt_matrix_poses=zeros(3, 3, length(gt_poses));
-for i=1:length(gt_poses)
-    gt_matrix_poses(:,:,i)=v2t([gt_poses(i).x gt_poses(i).y gt_poses(i).theta]);
-endfor
 
 %plot trajectory and landmarks
 myPlot(robot_poses,XR,gt_matrix_poses,init_guess_land,XL,gt_landmarks)

tools/algorithms/LeastSquaresUtils.m

@@ -103,13 +103,8 @@
 
 endfunction
 
-function [ep,J1,J2]=errorJacobianOdometry(transition)
-  next_pose=transition.pose_new;
-  prev_pose=transition.pose_prev;
-  trans=transition.v; 
+function [ep,J1,J2]=errorJacobianOdometry(P1,P2,trans)
 
-  P1=prev_pose;
-  P2=next_pose;
   p1=t2v(P1);
   p2=t2v(P2);
 
@@ -119,30 +114,39 @@
   c1=cos(theta1); s1=sin(theta1); c2=cos(theta2); s2=sin(theta2); 
   s12=sin(theta1 - theta2); c12=cos(theta1 - theta2);
 
+  %odometry measurement
   v=trans(1); t=trans(3);
 
+  %estimate next pose using current pose and odometry measurements
   p2_est=[x1+v*c1 y1+v*s1 theta1+t]';
   P2_est=v2t(p2_est);
 
-  #e=displacement I have - displacement I measure 
+  #error= displacement I have - displacement I measure 
   actual_disp=inv(P1)*P2;
-  e=actual_disp - inv(P1)*P2_est;
+  measure_disp=inv(P1)*P2_est;
+  ep=actual_disp - measure_disp;
+
 
-  %reshaping error to have a vector so J'* e can be computed
+  %reshaping error to have a vector so it cna be used to compute J'*e and so B
   %useless values are removed and it's possible to compute the jacobian using symbolic operations
-  ep=reshape(e(1:2,:),6,1);
+  ep=reshape(ep(1:2,:),6,1);
 
-  #jacobian 6x6
-J=[0,     0,              -s12,               0,  0,  s12;
-   0,     0,              -c12,               0,  0,  c12;
-   0,     0,               c12,               0,  0, -c12;
-   0,     0,              -s12,               0,  0,  s12;
-   -c1, -s1, y2*c1 - y1*c1 + x1*s1 - x2*s1,  c1, s1,   0;
-    s1, -c1, x1*c1 - x2*c1 + y1*s1 - y2*s1, -s1, c1,   0];
+
+  %derivative of error (6x1) w.r.t previoud and next state (6 variables)
+  %so jacobian is 6x6
+  Jp=[
+    0,     0,              -s12,               0,  0,  s12;
+    0,     0,              -c12,               0,  0,  c12;
+    0,     0,               c12,               0,  0, -c12;
+    0,     0,              -s12,               0,  0,  s12;
+    -c1, -s1, y2*c1 - y1*c1 + x1*s1 - x2*s1,  c1, s1,   0;
+     s1, -c1, x1*c1 - x2*c1 + y1*s1 - y2*s1, -s1, c1,   0];
 
 %6x3
-J1=J(1:6,1:3);
+J1=Jp(:,1:3);
+
 %6x3
-J2=J(1:6,4:6);
+J2=Jp(:,4:6);
+
 
 endfunction

tools/algorithms/evaluate.m

@@ -21,4 +21,15 @@
     endfor
     %computing final error
     err=err/length(est_landmarks);
+endfunction
+
+
+function err=evaluatePoses(gt_poses,est_poses)
+    err=0;
+    for i=1:length(gt_poses)
+        p1=t2v(gt_poses(:,:,i));
+        p2=t2v(est_poses(:,:,i));
+        err+=norm(p1-p2);
+    endfor
+    err=err/length(gt_poses);
 endfunction

tools/algorithms/initGuess.m

@@ -59,16 +59,16 @@
     % no real difference in using simply the poses or computing the trajectory using the transitions
 
     %compute trajectory using poses
-    robot_poses2=zeros(3, 3, length(poses));
+    robot_poses=zeros(3, 3, length(poses));
     for i=1:length(poses)
-        robot_poses2(:,:,i)=v2t([poses(i).x poses(i).y poses(i).theta]);
+        robot_poses(:,:,i)=v2t([poses(i).x poses(i).y poses(i).theta]');
     endfor
 
-
+    
     %compute trajectory using transitions
-    robot_poses=zeros(3, 3, length(poses));
-    robot_poses(:,:,1)=v2t([poses(1).x poses(1).y poses(1).theta]);
-    prev_pose=robot_poses(:,:,1);
+    robot_poses_odom=zeros(3, 3, length(poses));
+    robot_poses_odom(:,:,1)=v2t([poses(1).x poses(1).y poses(1).theta]);
+    prev_pose=robot_poses_odom(:,:,1);
     for i=1:length(transitions)
         V=transitions(i).v;
         displacement=v2t([V(1) V(2) V(3)]);
@@ -85,26 +85,27 @@
 
         #next_pose=displacement*prev_pose;
         prev_pose=next_pose;
-        robot_poses(:,:,i+1)=next_pose;
+        robot_poses_odom(:,:,i+1)=next_pose;
     endfor
 
     printf("Looking for differences between pose and pose using odometry measurements\n")
     total_dif=0;
     max_dif=-1;
     for i=1:length(robot_poses)
         p1=t2v(robot_poses(:,:,i));
-        p2=t2v(robot_poses2(:,:,i));
+        p2=t2v(robot_poses_odom(:,:,i));
         dif=norm(p1-p2);
         total_dif+=dif;
         if dif>max_dif
             max_dif=dif;
         endif
     endfor
     mean_dif=total_dif/length(robot_poses);
-    printf("Maximum difference is %f\nmean of differences is %f\n\n",max_dif,mean_dif)
+    printf("\tMaximum difference is %f\n\tmean of differences is %f\n\n",max_dif,mean_dif)
 
     #myDrawTrajectory(robot_poses,robot_poses,robot_poses2,length(robot_poses))
 
+    robot_poses=robot_poses_odom;
 
 
 endfunction

tools/algorithms/leastSquares_odom.m

@@ -0,0 +1,132 @@
+global pose_dim = 3; 
+global landmark_dim = 2;
+
+function [XR,XL]=leastSquares_odom(land_observations,robot_poses,landmark_positions,poses,T,gt_landmarks,gt_poses,params,land_id_to_idx)
+    global pose_dim;
+    global landmark_dim;
+    %parameters
+    kernel_threshold=params.kernel_threshold;
+    num_iterations=params.num_iterations;
+    damping=params.damping;
+
+    %system parameters
+    num_landmarks=length(land_observations);
+    num_poses=length(robot_poses);
+    system_size=pose_dim*num_poses+landmark_dim*num_landmarks; 
+
+    chi_stats   = zeros(1, num_iterations);
+    chi_stats_p   = zeros(1, num_iterations);
+    num_inliers = zeros(1, num_iterations);
+
+
+    XR=robot_poses;
+    XL=land_observations;
+    first_pose_id=params.first_pose_id;
+
+    A=get_association_matrix(land_observations,num_poses,params);       
+
+    for iteration=1:num_iterations
+        H=zeros(system_size, system_size);
+        b=zeros(system_size,1);
+        chi_stats(iteration) = 0;
+        chi_stats_p(iteration)=0;
+
+        %odometry measurement
+        for t=1:length(T)  
+            transition=T(t);
+
+            p_idx=(transition.id_from - first_pose_id)+1;
+            p_idx2=(transition.id_to - first_pose_id)+1;
+            prev_pose=XR(:,:,p_idx);
+            next_pose=XR(:,:,p_idx2);
+            v=transition.v;
+            [ep,J1,J2] = errorJacobianOdometry(prev_pose,next_pose,v);
+
+            chi=ep'*ep;
+            if (chi>kernel_threshold)
+                ep*=sqrt(kernel_threshold/chi);
+                chi=kernel_threshold;
+            else
+                num_inliers(iteration)++;
+            endif
+            chi_stats_p(iteration) += chi;
+
+            %Update H and B
+
+            p_m_1=poseMatrixIndex(p_idx, num_poses, num_landmarks);
+            p_m_2=poseMatrixIndex(p_idx2, num_poses, num_landmarks);
+
+            H(p_m_1:p_m_1+pose_dim-1,p_m_1:p_m_1+pose_dim-1) += J1'*J1;
+            H(p_m_1:p_m_1+pose_dim-1,p_m_2:p_m_2+pose_dim-1) += J1'*J2;
+            H(p_m_2:p_m_2+pose_dim-1,p_m_1:p_m_1+pose_dim-1) += (J1'*J2)';
+            H(p_m_2:p_m_2+pose_dim-1,p_m_2:p_m_2+pose_dim-1) += J2'*J2;
+
+            b(p_m_1:p_m_1 + pose_dim-1) += J1'*ep;
+            b(p_m_2:p_m_2+pose_dim-1) += J2'*ep;
+
+        endfor
+
+        %range measurements
+        %these two loops iterate through all the measurements
+        for p_idx=1:length(A)
+            for l_idx=1:length(land_observations)
+                Xr=XR(:,:,p_idx);
+                Xl=XL(l_idx).pos;
+                z=A(p_idx,l_idx);
+
+                if Xl && z>0
+                    %compute error and jacobian
+                    [e,Jr,Jl] = errorAndJacobian(Xr, Xl, z);
+                    chi=e'*e;
+                    if (chi>kernel_threshold)
+                        e*=sqrt(kernel_threshold/chi);
+                        chi=kernel_threshold;
+                    else
+                        num_inliers(iteration)++;
+                    endif
+                    chi_stats(iteration) += chi;
+
+                    %Update H and B
+                    pose_matrix_idx=poseMatrixIndex(p_idx, num_poses, num_landmarks);
+                    landmark_matrix_idx=landmarkMatrixIndex(l_idx, num_poses, num_landmarks);
+
+                    H(pose_matrix_idx:pose_matrix_idx+pose_dim-1,
+                    pose_matrix_idx:pose_matrix_idx+pose_dim-1) += (Jr'*Jr);
+
+                    H(pose_matrix_idx:pose_matrix_idx+pose_dim-1,
+                    landmark_matrix_idx:landmark_matrix_idx+landmark_dim-1) += (Jr'*Jl);
+
+                    H(landmark_matrix_idx:landmark_matrix_idx+landmark_dim-1,
+                    landmark_matrix_idx:landmark_matrix_idx+landmark_dim-1) += (Jl'*Jl);
+
+                    H(landmark_matrix_idx:landmark_matrix_idx+landmark_dim-1,
+                    pose_matrix_idx:pose_matrix_idx+pose_dim-1) += (Jr'*Jl)';
+
+                    b(pose_matrix_idx:pose_matrix_idx+pose_dim-1) += (Jr'*e);
+                    b(landmark_matrix_idx:landmark_matrix_idx+landmark_dim-1) += (Jl'*e);
+                endif
+            endfor
+
+        endfor
+
+        H += eye(system_size)*damping;
+        dx = zeros(system_size, 1);
+
+        %solve linear system
+        dx(pose_dim+1:end) = -(H(pose_dim+1:end, pose_dim+1:end) \ b(pose_dim+1:end, 1));
+        %apply found perturbation
+        [XR, XL] = boxPlus(XR, XL, num_poses, num_landmarks, dx);
+
+        %print error update
+        if mod(iteration,5)==0
+            printf("After %d iterations...\n",iteration)
+            printf("\t mean error in landmark position is %f\n",evaluate(gt_landmarks,XL,land_id_to_idx));
+            err_pose=evaluatePoses(gt_poses,XR);
+            printf("\t mean error in poses is %f\n",err_pose);
+        endif
+
+
+    endfor
+
+endfunction
+

tools/algorithms/poseFromId.m

@@ -14,3 +14,4 @@
     endfor
     %}
 endfunction
+