30 #include <pcl/common/eigen.h>
31 #include <pcl/common/common.h>
47 float r11,
float r12,
float r13,
float o14,
48 float r21,
float r22,
float r23,
float o24,
49 float r31,
float r32,
float r33,
float o34)
51 data_ = (cv::Mat_<float>(3,4) <<
59 UASSERT(transformationMatrix.cols == 4 &&
60 transformationMatrix.rows == 3 &&
61 (transformationMatrix.type() == CV_32FC1 || transformationMatrix.type() == CV_64FC1));
62 if(transformationMatrix.type() == CV_32FC1)
64 data_ = transformationMatrix;
68 transformationMatrix.convertTo(
data_, CV_32F);
79 data_(
cv::
Mat::zeros(3,4,CV_32FC1))
109 return (
data_.empty() ||
110 (
data()[0] == 0.0
f &&
120 data()[10] == 0.0
f &&
121 data()[11] == 0.0
f) ||
138 return data()[0] == 1.0f &&
148 data()[10] == 1.0f &&
171 bool invertible =
false;
173 Eigen::Matrix4f::RealScalar det;
180 bool invertible =
false;
182 Eigen::Matrix4f::RealScalar det;
241 return r13() == 0.0 &&
250 return data_.colRange(0, 3).clone();
255 return data_.col(3).clone();
278 Eigen::Vector3f
vA(
x,
y,
z);
280 return pcl::getAngle3D(Eigen::Vector4f(
vA[0],
vA[1],
vA[2], 0), Eigen::Vector4f(
vB[0],
vB[1],
vB[2], 0));
285 return uNorm(this->
x(), this->
y(), this->
z());
309 float x = this->
x() + t*(
other.x() - this->
x());
310 float y = this->
y() + t*(
other.y() - this->
y());
311 float z = this->
z() + t*(
other.z() - this->
z());
313 return Transform(x,
y,
z, qres.x(), qres.y(), qres.z(), qres.w());
330 return uFormat(
"xyz=[null] rpy=[null]");
336 return uFormat(
"xyz=%f,%f,%f rpy=%f,%f,%f",
x,
y,
z,
roll,
pitch,
yaw);
356 return memcmp(
data_.data,
t.data_.data,
data_.total() *
sizeof(
float)) == 0;
361 return !(*
this ==
t);
366 os <<
"[" <<
s.data()[0] <<
", " <<
s.data()[1] <<
", " <<
s.data()[2] <<
", " <<
s.data()[3] <<
";" << std::endl
367 <<
" " <<
s.data()[4] <<
", " <<
s.data()[5] <<
", " <<
s.data()[6] <<
", " <<
s.data()[7] <<
";" << std::endl
368 <<
" " <<
s.data()[8] <<
", " <<
s.data()[9] <<
", " <<
s.data()[10]<<
", " <<
s.data()[11] <<
"]";
413 return Transform(1,0,0,0, 0,1,0,0, 0,0,1,0);
477 std::list<std::string>
list =
uSplit(
string,
' ');
481 std::vector<float> numbers(
list.
size());
489 if(numbers.size() == 3)
491 t =
Transform(numbers[0], numbers[1], numbers[2]);
493 else if(numbers.size() == 6)
495 t =
Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
497 else if(numbers.size() == 7)
500 t =
Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5], numbers[6]);
502 else if(numbers.size() == 9)
504 t =
Transform(numbers[0], numbers[1], numbers[2], 0,
505 numbers[3], numbers[4], numbers[5], 0,
506 numbers[6], numbers[7], numbers[8], 0);
508 else if(numbers.size() == 12)
510 t =
Transform(numbers[0], numbers[1], numbers[2], numbers[3],
511 numbers[4], numbers[5], numbers[6], numbers[7],
512 numbers[8], numbers[9], numbers[10], numbers[11]);
526 std::list<std::string>
list =
uSplit(
string,
' ');
531 const std::map<double, Transform> & tfBuffer,
532 const double & stamp)
535 std::map<double, Transform>::const_iterator imuIterB = tfBuffer.lower_bound(stamp);
536 std::map<double, Transform>::const_iterator imuIterA = imuIterB;
537 if(imuIterA != tfBuffer.begin())
539 imuIterA = --imuIterA;
541 if(imuIterB == tfBuffer.end())
543 imuIterB = --imuIterB;
546 if(imuIterB->first == stamp)
548 imuT = imuIterB->second;
550 else if(imuIterA != imuIterB)
553 imuT = imuIterA->second.
interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
555 else if(stamp > imuIterB->first)
557 UWARN(
"No transform found for stamp %f! Latest is %f", stamp, imuIterB->first);
561 UWARN(
"No transform found for stamp %f! Earliest is %f", stamp, imuIterA->first);
567 const std::map<double, Transform> & tfBuffer,
568 const double & stamp,
572 std::map<double, Transform>::const_iterator imuIterB = tfBuffer.lower_bound(stamp);
573 std::map<double, Transform>::const_iterator imuIterA = imuIterB;
574 if(imuIterA != tfBuffer.begin())
576 imuIterA = --imuIterA;
578 if(imuIterB == tfBuffer.end())
580 imuIterB = --imuIterB;
583 if(imuIterB->first == stamp || imuIterA == imuIterB)
585 imuT = imuIterB->second;
588 *stampDiff =
fabs(imuIterB->first - stamp);
591 else if(imuIterA != imuIterB)
594 imuT = imuIterA->second.
interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);