00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap/core/Transform.h>
00029
00030 #include <pcl/common/eigen.h>
00031 #include <pcl/common/common.h>
00032 #include <rtabmap/core/util3d.h>
00033 #include <rtabmap/utilite/UConversion.h>
00034 #include <rtabmap/utilite/UMath.h>
00035 #include <rtabmap/utilite/ULogger.h>
00036 #include <rtabmap/utilite/UStl.h>
00037 #include <iomanip>
00038
00039 namespace rtabmap {
00040
00041 Transform::Transform() : data_(cv::Mat::zeros(3,4,CV_32FC1))
00042 {
00043 }
00044
00045
00046 Transform::Transform(
00047 float r11, float r12, float r13, float o14,
00048 float r21, float r22, float r23, float o24,
00049 float r31, float r32, float r33, float o34)
00050 {
00051 data_ = (cv::Mat_<float>(3,4) <<
00052 r11, r12, r13, o14,
00053 r21, r22, r23, o24,
00054 r31, r32, r33, o34);
00055 }
00056
00057 Transform::Transform(const cv::Mat & transformationMatrix)
00058 {
00059 UASSERT(transformationMatrix.cols == 4 &&
00060 transformationMatrix.rows == 3 &&
00061 (transformationMatrix.type() == CV_32FC1 || transformationMatrix.type() == CV_64FC1));
00062 if(transformationMatrix.type() == CV_32FC1)
00063 {
00064 data_ = transformationMatrix;
00065 }
00066 else
00067 {
00068 transformationMatrix.convertTo(data_, CV_32F);
00069 }
00070 }
00071
00072 Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
00073 {
00074 Eigen::Affine3f t = pcl::getTransformation (x, y, z, roll, pitch, yaw);
00075 *this = fromEigen3f(t);
00076 }
00077
00078 Transform::Transform(float x, float y, float z, float qx, float qy, float qz, float qw) :
00079 data_(cv::Mat::zeros(3,4,CV_32FC1))
00080 {
00081 Eigen::Matrix3f rotation = Eigen::Quaternionf(qw, qx, qy, qz).normalized().toRotationMatrix();
00082 data()[0] = rotation(0,0);
00083 data()[1] = rotation(0,1);
00084 data()[2] = rotation(0,2);
00085 data()[3] = x;
00086 data()[4] = rotation(1,0);
00087 data()[5] = rotation(1,1);
00088 data()[6] = rotation(1,2);
00089 data()[7] = y;
00090 data()[8] = rotation(2,0);
00091 data()[9] = rotation(2,1);
00092 data()[10] = rotation(2,2);
00093 data()[11] = z;
00094 }
00095
00096 Transform::Transform(float x, float y, float theta)
00097 {
00098 Eigen::Affine3f t = pcl::getTransformation (x, y, 0, 0, 0, theta);
00099 *this = fromEigen3f(t);
00100 }
00101
00102 Transform Transform::clone() const
00103 {
00104 return Transform(data_.clone());
00105 }
00106
00107 bool Transform::isNull() const
00108 {
00109 return (data_.empty() ||
00110 (data()[0] == 0.0f &&
00111 data()[1] == 0.0f &&
00112 data()[2] == 0.0f &&
00113 data()[3] == 0.0f &&
00114 data()[4] == 0.0f &&
00115 data()[5] == 0.0f &&
00116 data()[6] == 0.0f &&
00117 data()[7] == 0.0f &&
00118 data()[8] == 0.0f &&
00119 data()[9] == 0.0f &&
00120 data()[10] == 0.0f &&
00121 data()[11] == 0.0f) ||
00122 uIsNan(data()[0]) ||
00123 uIsNan(data()[1]) ||
00124 uIsNan(data()[2]) ||
00125 uIsNan(data()[3]) ||
00126 uIsNan(data()[4]) ||
00127 uIsNan(data()[5]) ||
00128 uIsNan(data()[6]) ||
00129 uIsNan(data()[7]) ||
00130 uIsNan(data()[8]) ||
00131 uIsNan(data()[9]) ||
00132 uIsNan(data()[10]) ||
00133 uIsNan(data()[11]));
00134 }
00135
00136 bool Transform::isIdentity() const
00137 {
00138 return data()[0] == 1.0f &&
00139 data()[1] == 0.0f &&
00140 data()[2] == 0.0f &&
00141 data()[3] == 0.0f &&
00142 data()[4] == 0.0f &&
00143 data()[5] == 1.0f &&
00144 data()[6] == 0.0f &&
00145 data()[7] == 0.0f &&
00146 data()[8] == 0.0f &&
00147 data()[9] == 0.0f &&
00148 data()[10] == 1.0f &&
00149 data()[11] == 0.0f;
00150 }
00151
00152 void Transform::setNull()
00153 {
00154 *this = Transform();
00155 }
00156
00157 void Transform::setIdentity()
00158 {
00159 *this = getIdentity();
00160 }
00161
00162 float Transform::theta() const
00163 {
00164 float roll, pitch, yaw;
00165 this->getEulerAngles(roll, pitch, yaw);
00166 return yaw;
00167 }
00168
00169 Transform Transform::inverse() const
00170 {
00171 return fromEigen4f(toEigen4f().inverse());
00172 }
00173
00174 Transform Transform::rotation() const
00175 {
00176 return Transform(
00177 data()[0], data()[1], data()[2], 0,
00178 data()[4], data()[5], data()[6], 0,
00179 data()[8], data()[9], data()[10], 0);
00180 }
00181
00182 Transform Transform::translation() const
00183 {
00184 return Transform(1,0,0, data()[3],
00185 0,1,0, data()[7],
00186 0,0,1, data()[11]);
00187 }
00188
00189 Transform Transform::to3DoF() const
00190 {
00191 float x,y,z,roll,pitch,yaw;
00192 this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
00193 return Transform(x,y,0, 0,0,yaw);
00194 }
00195
00196 cv::Mat Transform::rotationMatrix() const
00197 {
00198 return data_.colRange(0, 3).clone();
00199 }
00200
00201 cv::Mat Transform::translationMatrix() const
00202 {
00203 return data_.col(3).clone();
00204 }
00205
00206 void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
00207 {
00208 pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
00209 }
00210
00211 void Transform::getEulerAngles(float & roll, float & pitch, float & yaw) const
00212 {
00213 float x,y,z;
00214 pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
00215 }
00216
00217 void Transform::getTranslation(float & x, float & y, float & z) const
00218 {
00219 x = this->x();
00220 y = this->y();
00221 z = this->z();
00222 }
00223
00224 float Transform::getAngle(float x, float y, float z) const
00225 {
00226 Eigen::Vector3f vA(x,y,z);
00227 Eigen::Vector3f vB = this->toEigen3f().linear()*Eigen::Vector3f(1,0,0);
00228 return pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
00229 }
00230
00231 float Transform::getNorm() const
00232 {
00233 return uNorm(this->x(), this->y(), this->z());
00234 }
00235
00236 float Transform::getNormSquared() const
00237 {
00238 return uNormSquared(this->x(), this->y(), this->z());
00239 }
00240
00241 float Transform::getDistance(const Transform & t) const
00242 {
00243 return uNorm(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
00244 }
00245
00246 float Transform::getDistanceSquared(const Transform & t) const
00247 {
00248 return uNormSquared(this->x()-t.x(), this->y()-t.y(), this->z()-t.z());
00249 }
00250
00251 Transform Transform::interpolate(float t, const Transform & other) const
00252 {
00253 Eigen::Quaternionf qa=this->getQuaternionf();
00254 Eigen::Quaternionf qb=other.getQuaternionf();
00255 Eigen::Quaternionf qres = qa.slerp(t, qb);
00256
00257 float x = this->x() + t*(other.x() - this->x());
00258 float y = this->y() + t*(other.y() - this->y());
00259 float z = this->z() + t*(other.z() - this->z());
00260
00261 return Transform(x,y,z, qres.x(), qres.y(), qres.z(), qres.w());
00262 }
00263
00264 void Transform::normalizeRotation()
00265 {
00266 if(!this->isNull())
00267 {
00268 Eigen::Affine3f m = toEigen3f();
00269 m.linear() = Eigen::Quaternionf(m.linear()).normalized().toRotationMatrix();
00270 *this = fromEigen3f(m);
00271 }
00272 }
00273
00274 std::string Transform::prettyPrint() const
00275 {
00276 if(this->isNull())
00277 {
00278 return uFormat("xyz=[null] rpy=[null]");
00279 }
00280 else
00281 {
00282 float x,y,z,roll,pitch,yaw;
00283 getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
00284 return uFormat("xyz=%f,%f,%f rpy=%f,%f,%f", x,y,z, roll,pitch,yaw);
00285 }
00286 }
00287
00288 Transform Transform::operator*(const Transform & t) const
00289 {
00290 Eigen::Affine3f m = Eigen::Affine3f(toEigen4f()*t.toEigen4f());
00291
00292 m.linear() = Eigen::Quaternionf(m.linear()).normalized().toRotationMatrix();
00293 return fromEigen3f(m);
00294 }
00295
00296 Transform & Transform::operator*=(const Transform & t)
00297 {
00298 *this = *this * t;
00299 return *this;
00300 }
00301
00302 bool Transform::operator==(const Transform & t) const
00303 {
00304 return memcmp(data_.data, t.data_.data, data_.total() * sizeof(float)) == 0;
00305 }
00306
00307 bool Transform::operator!=(const Transform & t) const
00308 {
00309 return !(*this == t);
00310 }
00311
00312 std::ostream& operator<<(std::ostream& os, const Transform& s)
00313 {
00314 for(int i = 0; i < 3; ++i)
00315 {
00316 for(int j = 0; j < 4; ++j)
00317 {
00318 os << std::left << std::setw(12) << s.data()[i*4 + j] << " ";
00319 }
00320 os << std::endl;
00321 }
00322 return os;
00323 }
00324
00325 Eigen::Matrix4f Transform::toEigen4f() const
00326 {
00327 Eigen::Matrix4f m;
00328 m << data()[0], data()[1], data()[2], data()[3],
00329 data()[4], data()[5], data()[6], data()[7],
00330 data()[8], data()[9], data()[10], data()[11],
00331 0,0,0,1;
00332 return m;
00333 }
00334 Eigen::Matrix4d Transform::toEigen4d() const
00335 {
00336 Eigen::Matrix4d m;
00337 m << data()[0], data()[1], data()[2], data()[3],
00338 data()[4], data()[5], data()[6], data()[7],
00339 data()[8], data()[9], data()[10], data()[11],
00340 0,0,0,1;
00341 return m;
00342 }
00343
00344 Eigen::Affine3f Transform::toEigen3f() const
00345 {
00346 return Eigen::Affine3f(toEigen4f());
00347 }
00348
00349 Eigen::Affine3d Transform::toEigen3d() const
00350 {
00351 return Eigen::Affine3d(toEigen4d());
00352 }
00353
00354 Eigen::Quaternionf Transform::getQuaternionf() const
00355 {
00356 return Eigen::Quaternionf(this->toEigen3f().linear()).normalized();
00357 }
00358
00359 Eigen::Quaterniond Transform::getQuaterniond() const
00360 {
00361 return Eigen::Quaterniond(this->toEigen3d().linear()).normalized();
00362 }
00363
00364 Transform Transform::getIdentity()
00365 {
00366 return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
00367 }
00368
00369 Transform Transform::fromEigen4f(const Eigen::Matrix4f & matrix)
00370 {
00371 return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
00372 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
00373 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
00374 }
00375 Transform Transform::fromEigen4d(const Eigen::Matrix4d & matrix)
00376 {
00377 return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
00378 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
00379 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
00380 }
00381
00382 Transform Transform::fromEigen3f(const Eigen::Affine3f & matrix)
00383 {
00384 return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
00385 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
00386 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
00387 }
00388 Transform Transform::fromEigen3d(const Eigen::Affine3d & matrix)
00389 {
00390 return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
00391 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
00392 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
00393 }
00394
00395 Transform Transform::fromEigen3f(const Eigen::Isometry3f & matrix)
00396 {
00397 return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
00398 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
00399 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
00400 }
00401 Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
00402 {
00403 return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
00404 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
00405 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
00406 }
00407
00415 Transform Transform::fromString(const std::string & string)
00416 {
00417 std::list<std::string> list = uSplit(string, ' ');
00418 UASSERT_MSG(list.empty() || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12,
00419 uFormat("Cannot parse \"%s\"", string.c_str()).c_str());
00420
00421 std::vector<float> numbers(list.size());
00422 int i = 0;
00423 for(std::list<std::string>::iterator iter=list.begin(); iter!=list.end(); ++iter)
00424 {
00425 numbers[i++] = uStr2Float(*iter);
00426 }
00427
00428 Transform t;
00429 if(numbers.size() == 3)
00430 {
00431 t = Transform(numbers[0], numbers[1], numbers[2]);
00432 }
00433 else if(numbers.size() == 6)
00434 {
00435 t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
00436 }
00437 else if(numbers.size() == 7)
00438 {
00439
00440 t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5], numbers[6]);
00441 }
00442 else if(numbers.size() == 9)
00443 {
00444 t = Transform(numbers[0], numbers[1], numbers[2], 0,
00445 numbers[3], numbers[4], numbers[5], 0,
00446 numbers[6], numbers[7], numbers[8], 0);
00447 }
00448 else if(numbers.size() == 12)
00449 {
00450 t = Transform(numbers[0], numbers[1], numbers[2], numbers[3],
00451 numbers[4], numbers[5], numbers[6], numbers[7],
00452 numbers[8], numbers[9], numbers[10], numbers[11]);
00453 }
00454 return t;
00455 }
00456
00464 bool Transform::canParseString(const std::string & string)
00465 {
00466 std::list<std::string> list = uSplit(string, ' ');
00467 return list.size() == 0 || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12;
00468 }
00469
00470 }