add files

data/README.txt

@@ -0,0 +1,18 @@
+Project 03 - Range only SLAM
+
+The dataset is composed by a g2o file which contain poses and range observations. The file contain also odometry edges that are used to construct the initial guess
+for the problem. The use of those edges is your own choice if you think it's necessary.
+
+Hint :
+
+    - Parse the whole dataset and initialize the landmarks by using at least 3 range observations with the proper parallax
+    - Setup a LS optimization that involves all the poses and landmarks that you have initialized
+    - In the range edges the ID of the landmarkd is reported, use it to identify them for both the initialization and the global optimization
+
+        -EDGE_RANGE_SE2_XY id_pose id_landmark range
+
+Expected output :
+  - Robot trajectory and Map
+
+
+For any question concerning the initialization part feel free to contact me

main.m

@@ -0,0 +1,76 @@
+close all
+clear 
+clc
+clf
+
+addpath 'tools/g2o_wrapper'
+addpath 'tools/visualization'
+addpath 'tools/my_visualization'
+addpath 'tools/algorithms'
+source 'tools/algorithms/initGuess.m'
+source 'tools/algorithms/leastSquares.m'
+source 'tools/algorithms/LeastSquaresUtils.m'
+source 'tools/algorithms/evaluate.m'
+source 'tools/algorithms/tuning.m'
+source 'tools/my_visualization/myPlot.m'
+
+#get data
+data_path="data";
+dataset_gt="slam2d_range_only_ground_truth.g2o";
+dataset_ig="slam2d_range_only_initial_guess.g2o";
+
+printf("\n\nLoading Data...\n")
+[gt_landmarks, gt_poses, gt_transitions, gt_observations] = loadG2o(fullfile(data_path, dataset_gt));
+[~,poses,transitions,observations] = loadG2o(fullfile(data_path, dataset_ig));
+
+num_landmarks=length(gt_landmarks);
+num_poses=length(poses);
+num_transitions=length(transitions);
+num_observations=length(observations);
+printf("number of landmarks: %d\nnumber of poses: %d\nnumber of measurements: %d\n",num_landmarks,num_poses,num_observations)
+
+
+#Set some parameters
+first_pose_id=poses(1).id;
+params.first_pose_id=first_pose_id;
+
+params.kernel_threshold=2.0;
+params.num_iterations=25;
+params.damping=0.1;
+printf("parameters:\n")
+params
+
+#----INITIAL GUESS----
+printf("\n Initial Guess...\n")
+[landmark_positions,robot_poses,land_observations,land_id_to_idx]=initGuess(observations,poses,transitions,params);
+init_guess=land_observations;
+printf("Error in landmark position using initial guess is %f\n",evaluate(gt_landmarks,land_observations,land_id_to_idx));
+
+
+#---LEAST SQUARE OPTIMIZATION-------
+printf("\n Least squares using %d iterations...\n",params.num_iterations)
+[XR,XL]=leastSquares(land_observations,robot_poses,landmark_positions,poses,gt_landmarks,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
+#[XR,XL]=tuning(land_observations,robot_poses,landmark_positions,poses,gt_landmarks,params,land_id_to_idx);
+
+
+#----PLotting----
+printf("\nPlotting...\n")
+
+#to plot estimated landmarks and ground truth with labels
+#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,XL,gt_landmarks)
+

tools/algorithms/LeastSquaresUtils.m

@@ -0,0 +1,100 @@
+#size of pose and landmarks
+global pose_dim=3;
+global landmark_dim=2;
+
+#matrix of measurements
+#row:poses;   columns:landmarks;
+#so A(pose,landmark) correspond to the measure
+#used also to test if a different order of the measurements change the Least squares results
+function A=get_association_matrix(land_observations,num_poses,params)
+  landmark_num=length(land_observations);
+  A = ones(num_poses,landmark_num)*-1;
+  first_pose_id=params.first_pose_id;
+
+  #for each landmark 
+  for l_idx=1:length(land_observations)
+    land_obs=land_observations(l_idx);
+    #for each measure from the poses
+    for m=1:length(land_obs.measurements)
+      measurement=land_obs.measurements(m);
+      pose_id=measurement.obs_pose_id; 
+      pose_idx=pose_id-first_pose_id+1;
+      z=measurement.obs_range;
+      A(pose_idx,l_idx)=z;
+    endfor
+  endfor
+endfunction
+
+#retrieve index from H matrix
+function v_idx=poseMatrixIndex(pose_index, num_poses, num_landmarks)
+  global pose_dim;
+  global landmark_dim;
+
+  if (pose_index>num_poses)
+    printf("error matrix index: pose idx > num_poses\n")
+    v_idx=-1;
+    return;
+  endif;
+  v_idx=1+(pose_index-1)*pose_dim;
+endfunction;
+
+function v_idx=landmarkMatrixIndex(landmark_index, num_poses, num_landmarks)
+  global pose_dim;
+  global landmark_dim;
+  if (landmark_index>num_landmarks)
+    printf("error matrix index: land idx > num_landmark\n")
+    v_idx=-1;
+    return;
+  endif;
+  v_idx=1 + (num_poses)*pose_dim + (landmark_index-1) * landmark_dim;
+endfunction;
+
+#boxplus operator to add perturbation in manifold space
+function [XR, XL]=boxPlus(XR, XL, num_poses, num_landmarks, dx)
+  global pose_dim;
+  global landmark_dim;
+  for(pose_index=1:num_poses)
+    pose_matrix_index=poseMatrixIndex(pose_index, num_poses, num_landmarks);
+    dxr=dx(pose_matrix_index:pose_matrix_index+pose_dim-1);
+    #for poses we can't do simple addition, here is why boxplus is needed
+    XR(:,:,pose_index)=v2t(dxr)*XR(:,:,pose_index);
+
+  endfor;
+  for(landmark_index=1:num_landmarks)
+    #this is needed because some landmarks were not considered
+    if landmark_index<=length(XL) && XL(landmark_index).pos
+      landmark_matrix_index=landmarkMatrixIndex(landmark_index, num_poses, num_landmarks);
+      dxl=dx(landmark_matrix_index:landmark_matrix_index+landmark_dim-1,:);
+      #nothing special needed for landmark
+      XL(landmark_index).pos+=dxl;
+    endif
+  endfor;
+endfunction;
+
+
+function [e, Jr, Jl] = errorAndJacobian(Xr, Xl, z)
+    R = Xr(1:2,1:2);
+    t = Xr(1:2,3);
+
+    z_hat = norm( R' * (Xl-t));
+    #error
+    #estimated measurement - actual measurement
+    e = z_hat - z;
+
+    #computing jacobian
+    p=R' * (Xl-t);
+    c=Xr(1,1); s=Xr(2,1);
+    x=Xr(1,3); y=Xr(2,3);
+    xl=Xl(1);  yl=Xl(2);
+
+    #derivative of error with respect to state
+    f1=(c*x-c*xl+s*y-s*yl);  #recurrent term
+    f2=(-s*x+s*xl+c*y-c*yl); #other recurrent term
+    #Jr(3) is 0, error do not depends from angle!
+    #could have done with syms variables, but with this is faster and I forgot to use it before computing everything by hand
+
+    Jr=(1/norm(p)) * [f1*c+f2*-s, f1*s+f2*c, f1*( (-x*s)+(xl*s)+(y*c)+(-yl*c)) + f2*( (-x*c)+(xl*c)+(-y*s)+(yl*s)) ];
+    Jl=(1/norm(p)) * [f1*-c+f2*s f1*-s+f2*-c];
+
+
+endfunction

tools/algorithms/estimateLandmarkPosition.m

@@ -0,0 +1,38 @@
+
+#lateration algorithm to estimate position fo landmark
+#is a range based triangulation algorithm
+#different implementation possible, 
+#an implementation can be found here http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.7.933&rep=rep1&type=pdf
+#at page 504
+
+function [p1,p2]=estimateLandmarkPosition(measurements,poses)
+
+	num_measure=length(measurements);
+
+	obs_pose_id=measurements(num_measure).obs_pose_id;
+	last_pose=poseFromId(obs_pose_id,poses);
+    last_x = last_pose.x;
+	last_y = last_pose.y;
+	last_r = measurements(num_measure).obs_range;
+
+    A = zeros(num_measure-1, 2);
+    b = zeros(num_measure-1, 1);
+
+    for k=1:num_measure-1
+
+		obs_pose_id=measurements(k).obs_pose_id;
+		pose=poseFromId(obs_pose_id,poses);
+		x = pose.x;
+		y = pose.y;
+		r = measurements(k).obs_range;
+
+        A(k,:) = 2 * [(x - last_x) (y - last_y)];
+        b(k,:) = [x^2-last_x^2 + y^2-last_y^2 + last_r^2-r^2];
+    endfor
+
+    landmark_position = (A'*A) \ A'*b;
+
+    p1=landmark_position(1);
+    p2=landmark_position(2);
+
+end

tools/algorithms/evaluate.m

@@ -0,0 +1,24 @@
+1;
+function err=evaluate(gt_landmarks, est_landmarks,land_id_to_idx)
+    # error is computed as simply the distance between the correct 
+    # landmark and the estimated position
+    err=0;
+    for i=1:length(gt_landmarks)
+        #for each landmark retrieving original index and compute error
+        gt_id=gt_landmarks(i).id;
+        est_idx=land_id_to_idx(gt_id);
+        if gt_id==est_landmarks(est_idx).l_id
+            gt_pos=[ gt_landmarks(i).x_pose;  gt_landmarks(i).y_pose];
+            est_pos=est_landmarks(est_idx).pos;
+            if est_pos
+                err+=norm(gt_pos -est_pos);
+            endif
+        else 
+            #should not happen
+            printf("ERROR! indices do not correspond")
+            printf("  %d %d\n",gt_id,est_landmarks(est_idx).l_id)
+        endif
+    endfor
+    #computing final error
+    err=err/length(est_landmarks);
+endfunction

tools/algorithms/initGuess.m

@@ -0,0 +1,86 @@
+source 'tools/utilities/geometry_helpers_2d.m'
+
+function [landmark_positions,robot_poses,land_observations,l_id_to_idx] = initGuess(observations,poses,transitions,params)
+    #array of landmark_observations
+    #each landmark_observations is an array fo measures (pose,range)
+
+    first_pose_id=params.first_pose_id;
+
+    land_observations=[];
+
+    #used to transform landmark id into indeces 1...n
+    l_id_to_idx = ones(10000, 1)*-1;
+
+    #index for the landmarks
+    l_idx=1;
+
+    #building a structure that contains for each landmark all the measurement related to it.
+    for i=1:length(observations)
+        pose_id=observations(i).pose_id;
+        landmarks_observed=observations(i).observation;
+        for j=1:length(landmarks_observed)
+            observation=landmarks_observed(j);
+
+            #pose can be retrieved by the poses array, index is pose_id-first_pose_id+1
+            #obs_pose=poses(pose_id-first_pose_id+1);
+
+            #i want the landmarks to be stored independently from id but accessed fast
+            l_id=observation.id;
+            if l_id_to_idx(l_id)==-1
+                l_id_to_idx(l_id)=l_idx;
+                land_observations(l_id_to_idx(l_id)).l_id=l_id;
+                l_idx=l_idx+1;
+            endif
+
+            #adding this measurement
+            land_observations(l_id_to_idx(l_id)).measurements(end+1).obs_pose_id=pose_id;
+            land_observations(l_id_to_idx(l_id)).measurements(end).obs_range=observation.range;
+        endfor
+    endfor
+
+
+
+    #for each landmark with 3 or more measurements estimate position
+    for i=1:length(land_observations)
+        measurements=land_observations(i).measurements;
+
+        if length(measurements)>=3
+            [px py]=estimateLandmarkPosition(measurements,poses);
+            land_observations(i).pos=[px; py];
+            landmark_positions(end+1).id=i;
+            landmark_positions(end).pos=[px; py];
+        endif
+
+    endfor
+
+    #get poses in matrix form
+
+    # no real difference in using simply the poses or computing the trajectory using the transitions
+    #{
+    #compute trajectory using poses
+    robot_poses=zeros(3, 3, length(poses));
+    for i=1:length(poses)
+        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);
+    for i=1:length(transitions)
+        v=transitions(i).v;
+        displacement=v2t([v(1) v(2) v(3)]);
+        next_pose=prev_pose*displacement;
+        prev_pose=next_pose;
+        robot_poses(:,:,i+1)=next_pose;
+    endfor
+
+
+
+endfunction
+
+
+
+
+