| 
									
										
										
										
											2016-06-11 08:48:42 +08:00
										 |  |  | %% Add factors for all measurements | 
					
						
							| 
									
										
										
										
											2016-06-16 03:00:24 +08:00
										 |  |  | noise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma); | 
					
						
							|  |  |  | for i = 1:length(Z),  | 
					
						
							|  |  |  |     for k = 1:length(Z{i}) | 
					
						
							| 
									
										
										
										
											2016-06-11 08:48:42 +08:00
										 |  |  |         j = J{i}{k}; | 
					
						
							|  |  |  |         G.add(GenericProjectionFactorCal3_S2( | 
					
						
							| 
									
										
										
										
											2016-06-16 03:00:24 +08:00
										 |  |  |               Z{i}{k}, noise, symbol('x', i), symbol('p', j), K)); | 
					
						
							| 
									
										
										
										
											2016-06-11 08:48:42 +08:00
										 |  |  |     end | 
					
						
							|  |  |  | end |