Added matlab utility functions for plotting 3D points
							parent
							
								
									e2e780de4d
								
							
						
					
					
						commit
						1b935dbdc5
					
				|  | @ -0,0 +1,36 @@ | |||
| function plot3DPoints(values, linespec, marginals) | ||||
| %PLOT3DPOINTS Plots the Point3's in a values, with optional covariances | ||||
| %   Finds all the Point3 objects in the given Values object and plots them. | ||||
| % If a Marginals object is given, this function will also plot marginal | ||||
| % covariance ellipses for each point. | ||||
| 
 | ||||
| import gtsam.* | ||||
| 
 | ||||
| if ~exist('linespec', 'var') || isempty(linespec) | ||||
|     linespec = 'g'; | ||||
| end | ||||
| haveMarginals = exist('marginals', 'var'); | ||||
| keys = KeyVector(values.keys); | ||||
| 
 | ||||
| holdstate = ishold; | ||||
| hold on | ||||
| 
 | ||||
| % Plot points and covariance matrices | ||||
| for i = 0:keys.size-1 | ||||
|     key = keys.at(i); | ||||
|     p = values.at(key); | ||||
|     if isa(p, 'gtsam.Point3') | ||||
|         if haveMarginals | ||||
|             P = marginals.marginalCovariance(key); | ||||
|             plotPoint3(p, linespec, P); | ||||
|         else | ||||
|             plotPoint3(p, linespec); | ||||
|         end | ||||
|     end | ||||
| end | ||||
| 
 | ||||
| if ~holdstate | ||||
|     hold off | ||||
| end | ||||
| 
 | ||||
| end | ||||
|  | @ -0,0 +1,13 @@ | |||
| function plotPoint3(p, color, P) | ||||
| %PLOTPOINT3 Plot a Point3 with an optional covariance matrix | ||||
| if size(color,2)==1 | ||||
|     plot3(p.x,p.y,p.z,[color '*']); | ||||
| else | ||||
|     plot3(p.x,p.y,p.z,color); | ||||
| end | ||||
| if exist('P', 'var') | ||||
|     covarianceEllipse3D([p.x;p.y;p.z],P); | ||||
| end | ||||
| 
 | ||||
| end | ||||
| 
 | ||||
		Loading…
	
		Reference in New Issue