Formatting only
							parent
							
								
									8d54086f92
								
							
						
					
					
						commit
						ea0e16caf5
					
				
							
								
								
									
										192
									
								
								cpp/Pose3.h
								
								
								
								
							
							
						
						
									
										192
									
								
								cpp/Pose3.h
								
								
								
								
							|  | @ -12,111 +12,121 @@ | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /** A 3D pose (R,t) : (Rot3,Point3) */ | ||||
| class Pose3 { | ||||
| private: | ||||
| 	Rot3 R_; | ||||
| 	Point3 t_; | ||||
| 	/** A 3D pose (R,t) : (Rot3,Point3) */ | ||||
| 	class Pose3 { | ||||
| 	private: | ||||
| 		Rot3 R_; | ||||
| 		Point3 t_; | ||||
| 
 | ||||
| public: | ||||
| 	Pose3() {} // default is origin
 | ||||
| 	Pose3(const Pose3& pose):R_(pose.R_),t_(pose.t_) {} | ||||
| 	Pose3(const Rot3& R, const Point3& t): R_(R), t_(t) {} | ||||
| 	public: | ||||
| 		Pose3() { | ||||
| 		} // default is origin
 | ||||
| 		Pose3(const Pose3& pose) : | ||||
| 			R_(pose.R_), t_(pose.t_) { | ||||
| 		} | ||||
| 		Pose3(const Rot3& R, const Point3& t) : | ||||
| 			R_(R), t_(t) { | ||||
| 		} | ||||
| 
 | ||||
| 	/** constructor from 4*4 matrix */ | ||||
| 	Pose3(const Matrix &T) :R_( T(0,0), T(0,1), T(0,2), | ||||
| 			T(1,0), T(1,1), T(1,2), | ||||
| 			T(2,0), T(2,1), T(2,2) ), | ||||
| 			t_( T(0,3), T(1,3), T(2,3) ) {} | ||||
| 		/** constructor from 4*4 matrix */ | ||||
| 		Pose3(const Matrix &T) : | ||||
| 			R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), | ||||
| 					T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) { | ||||
| 		} | ||||
| 
 | ||||
| 	/** constructor from 12D vector */ | ||||
| 	Pose3(const Vector &V) :R_( V(0), V(3), V(6), | ||||
| 			V(1), V(4), V(7), | ||||
| 			V(2), V(5), V(8) ), | ||||
| 			t_( V(9), V(10), V(11) ) {} | ||||
| 		/** constructor from 12D vector */ | ||||
| 		Pose3(const Vector &V) : | ||||
| 			R_(V(0), V(3), V(6), V(1), V(4), V(7), V(2), V(5), V(8)), t_(V(9), V(10), | ||||
| 					V(11)) { | ||||
| 		} | ||||
| 
 | ||||
| 	const Rot3& rotation() const {return R_;} | ||||
| 		const Rot3& rotation() const { | ||||
| 			return R_; | ||||
| 		} | ||||
| 
 | ||||
| 	const Point3& translation() const {return t_;} | ||||
| 		const Point3& translation() const { | ||||
| 			return t_; | ||||
| 		} | ||||
| 
 | ||||
| 	/** return DOF, dimensionality of tangent space = 6 */ | ||||
| 	size_t dim() const { return 6;} | ||||
| 		/** return DOF, dimensionality of tangent space = 6 */ | ||||
| 		size_t dim() const { | ||||
| 			return 6; | ||||
| 		} | ||||
| 
 | ||||
| 	/** Given 6-dim tangent vector, create new pose */ | ||||
| 	Pose3 exmap(const Vector& d) const; | ||||
| 		/** Given 6-dim tangent vector, create new pose */ | ||||
| 		Pose3 exmap(const Vector& d) const; | ||||
| 
 | ||||
| 	/** inverse transformation */ | ||||
| 	Pose3 inverse() const; | ||||
| 		/** inverse transformation */ | ||||
| 		Pose3 inverse() const; | ||||
| 
 | ||||
| 	/** composition */ | ||||
| 	inline Pose3 operator*(const Pose3& B) const { | ||||
| 		return Pose3(matrix()*B.matrix()); | ||||
| 	} | ||||
| 		/** composition */ | ||||
| 		inline Pose3 operator*(const Pose3& B) const { | ||||
| 			return Pose3(matrix() * B.matrix()); | ||||
| 		} | ||||
| 
 | ||||
| 	/** return 12D vectorized form (column-wise) */ | ||||
| 	Vector vector() const { | ||||
| 		Vector r = R_.vector(), t = t_.vector(); | ||||
| 		return concatVectors(2,&r,&t); | ||||
| 	} | ||||
| 		/** return 12D vectorized form (column-wise) */ | ||||
| 		Vector vector() const { | ||||
| 			Vector r = R_.vector(), t = t_.vector(); | ||||
| 			return concatVectors(2, &r, &t); | ||||
| 		} | ||||
| 
 | ||||
| 	/** convert to 4*4 matrix */ | ||||
| 	Matrix matrix() const { | ||||
| 		const double row4[] = {0,0,0,1}; | ||||
| 		Matrix A34 = Matrix_(3,4,vector()), A14 = Matrix_(1,4,row4); | ||||
| 		return stack(2, &A34, &A14); | ||||
| 	} | ||||
| 		/** convert to 4*4 matrix */ | ||||
| 		Matrix matrix() const { | ||||
| 			const double row4[] = { 0, 0, 0, 1 }; | ||||
| 			Matrix A34 = Matrix_(3, 4, vector()), A14 = Matrix_(1, 4, row4); | ||||
| 			return stack(2, &A34, &A14); | ||||
| 		} | ||||
| 
 | ||||
| 	/** print with optional string */ | ||||
| 	void print(const std::string& s = "") const { | ||||
| 		R_.print(s+".R"); | ||||
| 		t_.print(s+".t"); | ||||
| 	} | ||||
| 		/** print with optional string */ | ||||
| 		void print(const std::string& s = "") const { | ||||
| 			R_.print(s + ".R"); | ||||
| 			t_.print(s + ".t"); | ||||
| 		} | ||||
| 
 | ||||
| 	/** transforms */ | ||||
| 	Pose3 transformPose_to(const Pose3& transform) const; | ||||
| 		/** transforms */ | ||||
| 		Pose3 transformPose_to(const Pose3& transform) const; | ||||
| 
 | ||||
| 		/** assert equality up to a tolerance */ | ||||
| 		bool equals(const Pose3& pose, double tol = 1e-9) const; | ||||
| 
 | ||||
| 		/** friends */ | ||||
| 		friend Point3 transform_from(const Pose3& pose, const Point3& p); | ||||
| 		friend Point3 transform_to(const Pose3& pose, const Point3& p); | ||||
| 		friend Pose3 composeTransform(const Pose3& current, const Pose3& target); | ||||
| 
 | ||||
| 	private: | ||||
| 		/** Serialization function */ | ||||
| 		friend class boost::serialization::access; | ||||
| 		template<class Archive> | ||||
| 		void serialize(Archive & ar, const unsigned int version) { | ||||
| 			ar & BOOST_SERIALIZATION_NVP(R_); | ||||
| 			ar & BOOST_SERIALIZATION_NVP(t_); | ||||
| 		} | ||||
| 	}; // Pose3 class
 | ||||
| 
 | ||||
| 	/** finds a transform from the current frame to the target frame given two coordinates of the same point */ | ||||
| 	Pose3 composeTransform(const Pose3& current, const Pose3& target); | ||||
| 
 | ||||
| 	/** receives the point in Pose coordinates and transforms it to world coordinates */ | ||||
| 	Point3 transform_from(const Pose3& pose, const Point3& p); | ||||
| 	Matrix Dtransform_from1(const Pose3& pose, const Point3& p); | ||||
| 	Matrix Dtransform_from2(const Pose3& pose); // does not depend on p !
 | ||||
| 
 | ||||
| 	/** receives the point in world coordinates and transforms it to Pose coordinates */ | ||||
| 	Point3 transform_to(const Pose3& pose, const Point3& p); | ||||
| 	Matrix Dtransform_to1(const Pose3& pose, const Point3& p); | ||||
| 	Matrix Dtransform_to2(const Pose3& pose); // does not depend on p !
 | ||||
| 
 | ||||
| 	/** direct measurement of a pose */ | ||||
| 	Vector hPose(const Vector& x); | ||||
| 
 | ||||
| 	/**
 | ||||
| 	 * derivative of direct measurement | ||||
| 	 * 12*6, entry i,j is how measurement error will change | ||||
| 	 */ | ||||
| 	Matrix DhPose(const Vector& x); | ||||
| 
 | ||||
| 	/** assert equality up to a tolerance */ | ||||
| 	bool equals(const Pose3& pose, double tol=1e-9) const; | ||||
| 
 | ||||
| 	/** friends */ | ||||
| 	friend Point3 transform_from(const Pose3& pose, const Point3& p); | ||||
| 	friend Point3 transform_to  (const Pose3& pose, const Point3& p); | ||||
| 	friend Pose3 composeTransform(const Pose3& current, const Pose3& target); | ||||
| 
 | ||||
| private: | ||||
| 	/** Serialization function */ | ||||
| 	friend class boost::serialization::access; | ||||
| 	template<class Archive> | ||||
| 	void serialize(Archive & ar, const unsigned int version) | ||||
| 	{ | ||||
| 		ar & BOOST_SERIALIZATION_NVP(R_); | ||||
| 		ar & BOOST_SERIALIZATION_NVP(t_); | ||||
| 	} | ||||
| }; // Pose3 class
 | ||||
| 
 | ||||
| /** finds a transform from the current frame to the target frame given two coordinates of the same point */ | ||||
| Pose3 composeTransform(const Pose3& current, const Pose3& target); | ||||
| 
 | ||||
| /** receives the point in Pose coordinates and transforms it to world coordinates */ | ||||
| Point3   transform_from (const Pose3& pose, const Point3& p); | ||||
| Matrix Dtransform_from1(const Pose3& pose, const Point3& p); | ||||
| Matrix Dtransform_from2(const Pose3& pose);  // does not depend on p !
 | ||||
| 
 | ||||
| /** receives the point in world coordinates and transforms it to Pose coordinates */ | ||||
| Point3   transform_to (const Pose3& pose, const Point3& p); | ||||
| Matrix Dtransform_to1(const Pose3& pose, const Point3& p); | ||||
| Matrix Dtransform_to2(const Pose3& pose);  // does not depend on p !
 | ||||
| 
 | ||||
| /** direct measurement of a pose */ | ||||
| Vector hPose (const Vector& x); | ||||
| 
 | ||||
| /**
 | ||||
|  * derivative of direct measurement | ||||
|  * 12*6, entry i,j is how measurement error will change | ||||
|  */ | ||||
| Matrix DhPose(const Vector& x); | ||||
| 
 | ||||
| /** assert equality up to a tolerance */ | ||||
| bool assert_equal(const Pose3& A, const Pose3& B, double tol=1e-9); | ||||
| 	bool assert_equal(const Pose3& A, const Pose3& B, double tol = 1e-9); | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue