Merged pcl_integration branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/src/Transform.cpp

@@ -0,0 +1,190 @@
+/*
+ * Transform.cpp
+ *
+ *  Created on: 2013-08-30
+ *      Author: Mathieu
+ */
+
+#include <rtabmap/core/Transform.h>
+
+#include <pcl/common/eigen.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UMath.h>
+#include <iomanip>
+
+namespace rtabmap {
+
+Transform::Transform() : data_(12)
+{
+	data_[0] = 0.0f;
+	data_[1] = 0.0f;
+	data_[2] = 0.0f;
+	data_[3] = 0.0f;
+	data_[4] = 0.0f;
+	data_[5] = 0.0f;
+	data_[6] = 0.0f;
+	data_[7] = 0.0f;
+	data_[8] = 0.0f;
+	data_[9] = 0.0f;
+	data_[10] = 0.0f;
+	data_[11] = 0.0f;
+}
+
+// rotation matrix r## and origin o##
+Transform::Transform(float r11, float r12, float r13, float o14,
+				     float r21, float r22, float r23, float o24,
+				     float r31, float r32, float r33, float o34) :
+	data_(12)
+{
+	data_[0] = r11;
+	data_[1] = r12;
+	data_[2] = r13;
+	data_[3] = o14;
+	data_[4] = r21;
+	data_[5] = r22;
+	data_[6] = r23;
+	data_[7] = o24;
+	data_[8] = r31;
+	data_[9] = r32;
+	data_[10] = r33;
+	data_[11] = o34;
+}
+
+Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
+{
+	Eigen::Affine3f t = pcl::getTransformation (x, y, z, roll, pitch, yaw);
+	*this = util3d::transformFromEigen3f(t);
+}
+
+bool Transform::isNull() const
+{
+	return (data_[0] == 0.0f &&
+			data_[1] == 0.0f &&
+			data_[2] == 0.0f &&
+			data_[3] == 0.0f &&
+			data_[4] == 0.0f &&
+			data_[5] == 0.0f &&
+			data_[6] == 0.0f &&
+			data_[7] == 0.0f &&
+			data_[8] == 0.0f &&
+			data_[9] == 0.0f &&
+			data_[10] == 0.0f &&
+			data_[11] == 0.0f) ||
+			uIsNan(data_[0]) ||
+			uIsNan(data_[1]) ||
+			uIsNan(data_[2]) ||
+			uIsNan(data_[3]) ||
+			uIsNan(data_[4]) ||
+			uIsNan(data_[5]) ||
+			uIsNan(data_[6]) ||
+			uIsNan(data_[7]) ||
+			uIsNan(data_[8]) ||
+			uIsNan(data_[9]) ||
+			uIsNan(data_[10]) ||
+			uIsNan(data_[11]);
+}
+
+bool Transform::isIdentity() const
+{
+	return data_[0] == 1.0f &&
+			data_[1] == 0.0f &&
+			data_[2] == 0.0f &&
+			data_[3] == 0.0f &&
+			data_[4] == 0.0f &&
+			data_[5] == 1.0f &&
+			data_[6] == 0.0f &&
+			data_[7] == 0.0f &&
+			data_[8] == 0.0f &&
+			data_[9] == 0.0f &&
+			data_[10] == 1.0f &&
+			data_[11] == 0.0f;
+}
+
+void Transform::setNull()
+{
+	*this = Transform();
+}
+
+void Transform::setIdentity()
+{
+	*this = getIdentity();
+}
+
+Transform Transform::getIdentity()
+{
+	return Transform(1,0,0,0,
+					 0,1,0,0,
+					 0,0,1,0);
+}
+
+Transform Transform::inverse() const
+{
+	Eigen::Matrix4f m = util3d::transformToEigen4f(*this);
+	return util3d::transformFromEigen4f(m.inverse());
+}
+
+Transform Transform::rotation() const
+{
+	return Transform(data_[0], data_[1], data_[2], 0,
+					 data_[4], data_[5], data_[6], 0,
+					 data_[8], data_[9], data_[10], 0);
+}
+
+Transform Transform::translation() const
+{
+	return Transform(1,0,0, data_[3],
+					 0,1,0, data_[7],
+					 0,0,1, data_[11]);
+}
+
+void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
+{
+	pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(*this), x, y, z, roll, pitch, yaw);
+}
+
+std::string Transform::prettyPrint() const
+{
+	float x,y,z,roll,pitch,yaw;
+	getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+	return uFormat("xyz=%f,%f,%f rpy=%f,%f,%f", x,y,z, roll,pitch,yaw);
+}
+
+Transform Transform::operator*(const Transform & t) const
+{
+	Eigen::Matrix4f m1 = util3d::transformToEigen4f(*this);
+	Eigen::Matrix4f m2 = util3d::transformToEigen4f(t);
+	return util3d::transformFromEigen4f(m1*m2);
+}
+
+Transform & Transform::operator*=(const Transform & t)
+{
+	*this = *this * t;
+	return *this;
+}
+
+bool Transform::operator==(const Transform & t) const
+{
+	return memcmp(data_.data(), t.data_.data(), data_.size() * sizeof(float)) == 0;
+}
+
+bool Transform::operator!=(const Transform & t) const
+{
+	return !(*this == t);
+}
+
+std::ostream& operator<<(std::ostream& os, const Transform& s)
+{
+	for(int i = 0; i < 3; ++i)
+	{
+		for(int j = 0; j < 4; ++j)
+		{
+			std::cout << std::left << std::setw(12) << s.data()[i*4 + j];
+		}
+		std::cout << std::endl;
+	}
+	return os;
+}
+
+
+}