Comment cleanup to /** style and adding global print functions
							parent
							
								
									78a579a24a
								
							
						
					
					
						commit
						1dcc864d97
					
				
							
								
								
									
										50
									
								
								.cproject
								
								
								
								
							
							
						
						
									
										50
									
								
								.cproject
								
								
								
								
							|  | @ -1,4 +1,4 @@ | |||
| <?xml version="1.0" encoding="UTF-8" standalone="no"?> | ||||
| <?xml version="1.0" encoding="UTF-8"?> | ||||
| <?fileVersion 4.0.0?> | ||||
| 
 | ||||
| <cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage"> | ||||
|  | @ -298,22 +298,6 @@ | |||
| 			</storageModule> | ||||
| 			<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets"> | ||||
| <buildTargets> | ||||
| <target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| <buildCommand>make</buildCommand> | ||||
| <buildArguments>-j2</buildArguments> | ||||
| <buildTarget>install</buildTarget> | ||||
| <stopOnError>true</stopOnError> | ||||
| <useDefaultCommand>true</useDefaultCommand> | ||||
| <runAllBuilders>true</runAllBuilders> | ||||
| </target> | ||||
| <target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| <buildCommand>make</buildCommand> | ||||
| <buildArguments>-j2</buildArguments> | ||||
| <buildTarget>check</buildTarget> | ||||
| <stopOnError>true</stopOnError> | ||||
| <useDefaultCommand>true</useDefaultCommand> | ||||
| <runAllBuilders>true</runAllBuilders> | ||||
| </target> | ||||
| <target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| <buildCommand>make</buildCommand> | ||||
| <buildArguments>-k</buildArguments> | ||||
|  | @ -468,7 +452,7 @@ | |||
| </target> | ||||
| <target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| <buildCommand>make</buildCommand> | ||||
| <buildArguments/> | ||||
| <buildArguments></buildArguments> | ||||
| <buildTarget>testBayesTree.run</buildTarget> | ||||
| <stopOnError>true</stopOnError> | ||||
| <useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -483,7 +467,7 @@ | |||
| </target> | ||||
| <target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| <buildCommand>make</buildCommand> | ||||
| <buildArguments/> | ||||
| <buildArguments></buildArguments> | ||||
| <buildTarget>testSymbolicFactorGraph.run</buildTarget> | ||||
| <stopOnError>true</stopOnError> | ||||
| <useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -719,18 +703,10 @@ | |||
| <useDefaultCommand>true</useDefaultCommand> | ||||
| <runAllBuilders>true</runAllBuilders> | ||||
| </target> | ||||
| <target name="testPose3Config.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| <target name="testLieConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| <buildCommand>make</buildCommand> | ||||
| <buildArguments>-j2</buildArguments> | ||||
| <buildTarget>testPose3Config.run</buildTarget> | ||||
| <stopOnError>true</stopOnError> | ||||
| <useDefaultCommand>true</useDefaultCommand> | ||||
| <runAllBuilders>true</runAllBuilders> | ||||
| </target> | ||||
| <target name="testPose3Graph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| <buildCommand>make</buildCommand> | ||||
| <buildArguments>-j2</buildArguments> | ||||
| <buildTarget>testPose3Graph.run</buildTarget> | ||||
| <buildTarget>testLieConfig.run</buildTarget> | ||||
| <stopOnError>true</stopOnError> | ||||
| <useDefaultCommand>true</useDefaultCommand> | ||||
| <runAllBuilders>true</runAllBuilders> | ||||
|  | @ -759,6 +735,22 @@ | |||
| <useDefaultCommand>true</useDefaultCommand> | ||||
| <runAllBuilders>true</runAllBuilders> | ||||
| </target> | ||||
| <target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| <buildCommand>make</buildCommand> | ||||
| <buildArguments>-j2</buildArguments> | ||||
| <buildTarget>install</buildTarget> | ||||
| <stopOnError>true</stopOnError> | ||||
| <useDefaultCommand>true</useDefaultCommand> | ||||
| <runAllBuilders>true</runAllBuilders> | ||||
| </target> | ||||
| <target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| <buildCommand>make</buildCommand> | ||||
| <buildArguments>-j2</buildArguments> | ||||
| <buildTarget>check</buildTarget> | ||||
| <stopOnError>true</stopOnError> | ||||
| <useDefaultCommand>true</useDefaultCommand> | ||||
| <runAllBuilders>true</runAllBuilders> | ||||
| </target> | ||||
| </buildTargets> | ||||
| </storageModule> | ||||
| </cconfiguration> | ||||
|  |  | |||
|  | @ -75,7 +75,7 @@ namespace gtsam { | |||
|   /** Call equal on the object without tolerance (use default tolerance) */ | ||||
|   template<class T> | ||||
|   inline bool equal(const T& obj1, const T& obj2) { | ||||
|     return obj1.equal(obj2); | ||||
|     return obj1.equals(obj2); | ||||
|   } | ||||
| 
 | ||||
|   // Vector Group operations
 | ||||
|  |  | |||
							
								
								
									
										16
									
								
								cpp/Point2.h
								
								
								
								
							
							
						
						
									
										16
									
								
								cpp/Point2.h
								
								
								
								
							|  | @ -62,18 +62,22 @@ namespace gtsam { | |||
|     } | ||||
|   }; | ||||
| 
 | ||||
|   // Lie group functions
 | ||||
|   /** Lie group functions */ | ||||
| 
 | ||||
|   // Dimensionality of the tangent space
 | ||||
|   /** Global print calls member function */ | ||||
|   inline void print(const Point2& p, std::string& s) { p.print(s); } | ||||
|   inline void print(const Point2& p) { p.print(); } | ||||
| 
 | ||||
|   /** Dimensionality of the tangent space */ | ||||
|   inline size_t dim(const Point2& obj) { return 2; } | ||||
| 
 | ||||
|   // Exponential map around identity - just create a Point2 from a vector
 | ||||
|   /** Exponential map around identity - just create a Point2 from a vector */ | ||||
|   template<> inline Point2 expmap(const Vector& dp) { return Point2(dp); } | ||||
| 
 | ||||
|   // Log map around identity - just return the Point2 as a vector
 | ||||
|   /** Log map around identity - just return the Point2 as a vector */ | ||||
|   inline Vector logmap(const Point2& dp) { return Vector_(2, dp.x(), dp.y()); } | ||||
| 
 | ||||
|   // "Compose", just adds the coordinates of two points.
 | ||||
|   /** "Compose", just adds the coordinates of two points. */ | ||||
|   inline Point2 compose(const Point2& p1, const Point2& p0) { return p0+p1; } | ||||
|   inline Matrix Dcompose1(const Point2& p1, const Point2& p0) { | ||||
|     return Matrix_(2,2, | ||||
|  | @ -84,7 +88,7 @@ namespace gtsam { | |||
|         1.0, 0.0, | ||||
|         0.0, 1.0); } | ||||
| 
 | ||||
|   // "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2()
 | ||||
|   /** "Inverse" - negates each coordinate such that compose(p,inverse(p))=Point2() */ | ||||
|   inline Point2 inverse(const Point2& p) { return Point2(-p.x(), -p.y()); } | ||||
| 
 | ||||
| } | ||||
|  |  | |||
							
								
								
									
										16
									
								
								cpp/Point3.h
								
								
								
								
							
							
						
						
									
										16
									
								
								cpp/Point3.h
								
								
								
								
							|  | @ -78,18 +78,20 @@ namespace gtsam { | |||
|   }; | ||||
| 
 | ||||
| 
 | ||||
|   /** return DOF, dimensionality of tangent space */ | ||||
|   /** Global print calls member function */ | ||||
|   inline void print(const Point3& p, std::string& s) { p.print(s); } | ||||
|   inline void print(const Point3& p) { p.print(); } | ||||
| 
 | ||||
|   // Dimensionality of the tangent space
 | ||||
|   /** return DOF, dimensionality of tangent space */ | ||||
|   inline size_t dim(const Point3&) { return 3; } | ||||
| 
 | ||||
|   // Exponential map at identity - just create a Point3 from x,y,z
 | ||||
|   /** Exponential map at identity - just create a Point3 from x,y,z */ | ||||
|   template<> inline Point3 expmap(const Vector& dp) { return Point3(dp); } | ||||
| 
 | ||||
|   // Log map at identity - return the x,y,z of this point
 | ||||
|   /** Log map at identity - return the x,y,z of this point */ | ||||
|   inline Vector logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); } | ||||
| 
 | ||||
|   // "Compose" - just adds coordinates of two points
 | ||||
|   /** "Compose" - just adds coordinates of two points */ | ||||
|   inline Point3 compose(const Point3& p1, const Point3& p0) { return p0+p1; } | ||||
|   inline Matrix Dcompose1(const Point3& p1, const Point3& p0) { | ||||
|     return Matrix_(3,3, | ||||
|  | @ -104,11 +106,11 @@ namespace gtsam { | |||
|         0.0, 0.0, 1.0); | ||||
|   } | ||||
| 
 | ||||
|   // "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3()
 | ||||
|   /** "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() */ | ||||
|   inline Point3 inverse(const Point3& p) { return Point3(-p.x(), -p.y(), -p.z()); } | ||||
| 
 | ||||
| 
 | ||||
|   // Syntactic sugar for multiplying coordinates by a scalar s*p
 | ||||
|   /** Syntactic sugar for multiplying coordinates by a scalar s*p */ | ||||
|   inline Point3 operator*(double s, const Point3& p) { return p*s;} | ||||
| 
 | ||||
|   /** add two points, add(p,q) is same as p+q */ | ||||
|  |  | |||
							
								
								
									
										26
									
								
								cpp/Pose3.h
								
								
								
								
							
							
						
						
									
										26
									
								
								cpp/Pose3.h
								
								
								
								
							|  | @ -68,42 +68,42 @@ namespace gtsam { | |||
|     } | ||||
|   }; // Pose3 class
 | ||||
| 
 | ||||
|   // global print
 | ||||
|   /** global print */ | ||||
|   inline void print(const Pose3& p, const std::string& s = "") { p.print(s);} | ||||
| 
 | ||||
|   // Dimensionality of the tangent space
 | ||||
|   /** Dimensionality of the tangent space */ | ||||
|   inline size_t dim(const Pose3&) { return 6; } | ||||
| 
 | ||||
|   // Compose two poses
 | ||||
|   /** Compose two poses */ | ||||
|   inline Pose3 compose(const Pose3& p0, const Pose3& p1) { | ||||
|     return Pose3(p0.rotation()*p1.rotation(), | ||||
|         p0.translation() + p0.rotation()*p1.translation()); | ||||
|   } | ||||
| 
 | ||||
|   // Find the inverse pose s.t. inverse(p)*p = Pose3()
 | ||||
|   /** Find the inverse pose s.t. inverse(p)*p = Pose3() */ | ||||
|   inline Pose3 inverse(const Pose3& p) { | ||||
|     Rot3 Rt = inverse(p.rotation()); | ||||
|     return Pose3(Rt, Rt*(-p.translation())); | ||||
|   } | ||||
| 
 | ||||
|   // Exponential map at identity - create a pose with a translation and
 | ||||
|   // rotation (in canonical coordinates)
 | ||||
|   /** Exponential map at identity - create a pose with a translation and
 | ||||
|    * rotation (in canonical coordinates). */ | ||||
|   template<> Pose3 expmap(const Vector& d); | ||||
| 
 | ||||
|   // Log map at identity - return the translation and canonical rotation
 | ||||
|   // coordinates of a pose.
 | ||||
|   /** Log map at identity - return the translation and canonical rotation
 | ||||
|    * coordinates of a pose. */ | ||||
|   Vector logmap(const Pose3& p); | ||||
| 
 | ||||
|   // todo: these are the "old-style" expmap and logmap about the specified
 | ||||
|   // pose.
 | ||||
|   // Increments the offset and rotation independently given a translation and
 | ||||
|   // canonical rotation coordinates
 | ||||
|   /** todo: these are the "old-style" expmap and logmap about the specified
 | ||||
|    * pose. | ||||
|    * Increments the offset and rotation independently given a translation and | ||||
|    * canonical rotation coordinates */ | ||||
|   template<> inline Pose3 expmap<Pose3>(const Pose3& p0, const Vector& d) { | ||||
|     return Pose3(expmap(p0.rotation(), sub(d, 0, 3)), | ||||
|         expmap(p0.translation(), sub(d, 3, 6))); | ||||
|   } | ||||
| 
 | ||||
|   // Independently computes the logmap of the translation and rotation.
 | ||||
|   /** Independently computes the logmap of the translation and rotation. */ | ||||
|   template<> inline Vector logmap<Pose3>(const Pose3& p0, const Pose3& pp) { | ||||
|     const Vector r(logmap(p0.rotation(), pp.rotation())), | ||||
|         t(logmap(p0.translation(), pp.translation())); | ||||
|  |  | |||
							
								
								
									
										18
									
								
								cpp/Rot2.h
								
								
								
								
							
							
						
						
									
										18
									
								
								cpp/Rot2.h
								
								
								
								
							|  | @ -77,36 +77,40 @@ namespace gtsam { | |||
| 
 | ||||
|   // Lie group functions
 | ||||
| 
 | ||||
|   // Dimensionality of the tangent space
 | ||||
|   /** Global print calls member function */ | ||||
|   inline void print(const Rot2& r, std::string& s) { r.print(s); } | ||||
|   inline void print(const Rot2& r) { r.print(); } | ||||
| 
 | ||||
|   /** Dimensionality of the tangent space */ | ||||
|   inline size_t dim(const Rot2&) { return 1; } | ||||
| 
 | ||||
|   // Expmap around identity - create a rotation from an angle
 | ||||
|   /** Expmap around identity - create a rotation from an angle */ | ||||
|   template<> inline Rot2 expmap(const Vector& v) { | ||||
|     if (zero(v)) return (Rot2()); | ||||
|     else return Rot2(v(0)); | ||||
|   } | ||||
| 
 | ||||
|   // Logmap around identity - return the angle of the rotation
 | ||||
|   /** Logmap around identity - return the angle of the rotation */ | ||||
|   inline Vector logmap(const Rot2& r) { | ||||
|     return Vector_(1, r.theta()); | ||||
|   } | ||||
| 
 | ||||
|   // Compose - make a new rotation by adding angles
 | ||||
|   /** Compose - make a new rotation by adding angles */ | ||||
|   inline Rot2 compose(const Rot2& r0, const Rot2& r1) { | ||||
|     return Rot2( | ||||
|         r0.c() * r1.c() - r0.s() * r1.s(), | ||||
|         r0.s() * r1.c() + r0.c() * r1.s()); | ||||
|   } | ||||
| 
 | ||||
|   // Syntactic sugar R1*R2 = compose(R1,R2)
 | ||||
|   /** Syntactic sugar R1*R2 = compose(R1,R2) */ | ||||
|   inline Rot2 operator*(const Rot2& r0, const Rot2& r1) { | ||||
|     return compose(r0, r1); | ||||
|   } | ||||
| 
 | ||||
|   // The inverse rotation - negative angle
 | ||||
|   /** The inverse rotation - negative angle */ | ||||
|   inline Rot2 inverse(const Rot2& r) { return Rot2(r.c(), -r.s());} | ||||
| 
 | ||||
|   // Shortcut to compose the inverse: invcompose(R0,R1) = inverse(R0)*R1
 | ||||
|   /** Shortcut to compose the inverse: invcompose(R0,R1) = inverse(R0)*R1 */ | ||||
|   inline Rot2 invcompose(const Rot2& r0, const Rot2& r1) { | ||||
|     return Rot2( | ||||
|          r0.c() * r1.c() + r0.s() * r1.s(), | ||||
|  |  | |||
|  | @ -117,6 +117,9 @@ namespace gtsam { | |||
|     } | ||||
|   }; | ||||
| 
 | ||||
|   /** Global print calls member function */ | ||||
|   inline void print(const Rot3& r, std::string& s) { r.print(s); } | ||||
|   inline void print(const Rot3& r) { r.print(); } | ||||
| 
 | ||||
|   /**
 | ||||
|    * Rodriguez' formula to compute an incremental rotation matrix | ||||
|  |  | |||
|  | @ -110,11 +110,15 @@ TEST(LieConfig, expmap_d) | |||
|   config0.insert("v1", Vector_(3, 1.0, 2.0, 3.0)); | ||||
|   config0.insert("v2", Vector_(3, 5.0, 6.0, 7.0)); | ||||
|   //config0.print("config0");
 | ||||
|   CHECK(equal(config0, config0)); | ||||
|   CHECK(config0.equals(config0)); | ||||
| 
 | ||||
|   LieConfig<Pose2> poseconfig; | ||||
|   poseconfig.insert("p1", Pose2(1,2,3)); | ||||
|   poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); | ||||
|   //poseconfig.print("poseconfig");
 | ||||
|   CHECK(equal(config0, config0)); | ||||
|   CHECK(config0.equals(config0)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue