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
							 |