Formatting only

cpp/Pose3.h

@@ -12,35 +12,46 @@
 
 namespace gtsam {
 
-/** A 3D pose (R,t) : (Rot3,Point3) */
+	/** A 3D pose (R,t) : (Rot3,Point3) */
-class Pose3 {
+	class Pose3 {
-private:
+	private:
 		Rot3 R_;
 		Point3 t_;
 
-public:
+	public:
-	Pose3() {} // default is origin
+		Pose3() {
-	Pose3(const Pose3& pose):R_(pose.R_),t_(pose.t_) {}
+		} // default is origin
-	Pose3(const Rot3& R, const Point3& t): R_(R), t_(t) {}
+		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),
+		Pose3(const Matrix &T) :
-			T(1,0), T(1,1), T(1,2),
+			R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0),
-			T(2,0), T(2,1), T(2,2) ),
+					T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
-			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),
+		Pose3(const Vector &V) :
-			V(1), V(4), V(7),
+			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(2), V(5), V(8) ),
+					V(11)) {
-			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;}
+		size_t dim() const {
+			return 6;
+		}
 
 		/** Given 6-dim tangent vector, create new pose */
 		Pose3 exmap(const Vector& d) const;
@@ -50,73 +61,72 @@ public:
 
 		/** composition */
 		inline Pose3 operator*(const Pose3& B) const {
-		return Pose3(matrix()*B.matrix());
+			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 concatVectors(2, &r, &t);
 		}
 
 		/** convert to 4*4 matrix */
 		Matrix matrix() const {
-		const double row4[] = {0,0,0,1};
+			const double row4[] = { 0, 0, 0, 1 };
-		Matrix A34 = Matrix_(3,4,vector()), A14 = Matrix_(1,4,row4);
+			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");
+			R_.print(s + ".R");
-		t_.print(s+".t");
+			t_.print(s + ".t");
 		}
 
 		/** transforms */
 		Pose3 transformPose_to(const Pose3& transform) const;
 
 		/** assert equality up to a tolerance */
-	bool equals(const Pose3& pose, double tol=1e-9) const;
+		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 Point3 transform_to(const Pose3& pose, const Point3& p);
 		friend Pose3 composeTransform(const Pose3& current, const Pose3& target);
 
-private:
+	private:
 		/** Serialization function */
 		friend class boost::serialization::access;
 		template<class Archive>
-	void serialize(Archive & ar, const unsigned int version)
+		void serialize(Archive & ar, const unsigned int version) {
-	{
 			ar & BOOST_SERIALIZATION_NVP(R_);
 			ar & BOOST_SERIALIZATION_NVP(t_);
 		}
-}; // Pose3 class
+	}; // Pose3 class
 
-/** finds a transform from the current frame to the target frame given two coordinates of the same point */
+	/** 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);
+	Pose3 composeTransform(const Pose3& current, const Pose3& target);
 
-/** receives the point in Pose coordinates and transforms it to world coordinates */
+	/** receives the point in Pose coordinates and transforms it to world coordinates */
-Point3   transform_from (const Pose3& pose, const Point3& p);
+	Point3 transform_from(const Pose3& pose, const Point3& p);
-Matrix Dtransform_from1(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 !
+	Matrix Dtransform_from2(const Pose3& pose); // does not depend on p !
 
-/** receives the point in world coordinates and transforms it to Pose coordinates */
+	/** receives the point in world coordinates and transforms it to Pose coordinates */
-Point3   transform_to (const Pose3& pose, const Point3& p);
+	Point3 transform_to(const Pose3& pose, const Point3& p);
-Matrix Dtransform_to1(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 !
+	Matrix Dtransform_to2(const Pose3& pose); // does not depend on p !
 
-/** direct measurement of a pose */
+	/** direct measurement of a pose */
-Vector hPose (const Vector& x);
+	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);
+	Matrix DhPose(const Vector& x);
 
-/** assert equality up to a tolerance */
+	/** 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