static void calculateDirectedTransform(const Eigen::Vector3d& start,
const Eigen::Vector3d& stop,
const tf::Vector3 z_axis,
tf::Transform& transform)
{
tf::Vector3 origin;
tf::Quaternion pose;
origin.setX(start(1));
origin.setY(start(0));
origin.setZ(start(2) - 0.05);
transform.setOrigin(origin);
tf::Vector3 n = toTF(stop) - toTF(start);
n.normalize();
tf::Vector3 z = z_axis;
tf::Vector3 y;
y = n.cross(z);
y.normalize();
z = n.cross(y);
tf::Matrix3x3 rotation(
z.getX(), n.getX(), y.getX(),
z.getY(), n.getY(), y.getY(),
z.getZ(), n.getZ(), y.getZ());
transform.setBasis(rotation);
}
void transformEigenToTF(const Eigen::Affine3d &e, tf::Transform &t)
{
t.setOrigin(tf::Vector3( e.matrix()(0,3), e.matrix()(1,3), e.matrix()(2,3)));
t.setBasis(tf::Matrix3x3(e.matrix()(0,0), e.matrix()(0,1),e.matrix()(0,2),
e.matrix()(1,0), e.matrix()(1,1),e.matrix()(1,2),
e.matrix()(2,0), e.matrix()(2,1),e.matrix()(2,2)));
}
void transformKDLToTF(const KDL::Frame &k, tf::Transform &t)
{
t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
k.M.data[3], k.M.data[4], k.M.data[5],
k.M.data[6], k.M.data[7], k.M.data[8]));
}
void eigenMatrix4fToTransform(Eigen::Matrix4f &m, tf::Transform &t)
{
tf::Matrix3x3 basis = tf::Matrix3x3(m(0,0), m(0,1), m(0,2),
m(1,0), m(1,1), m(1,2),
m(2,0), m(2,1), m(2,2));
tf::Vector3 origin = tf::Vector3(m(0,3), m(1,3), m(2,3));
t.setBasis(basis);
t.setOrigin(origin);
}
void openCVRtToTf(
const cv::Mat& R,
const cv::Mat& t,
tf::Transform& transform)
{
tf::Vector3 translation_tf(
t.at<double>(0,0),
t.at<double>(1,0),
t.at<double>(2,0));
tf::Matrix3x3 rotation_tf;
for(int i = 0; i < 3; ++i)
for(int j = 0; j < 3; ++j)
rotation_tf[j][i] = R.at<double>(j,i);
transform.setOrigin(translation_tf);
transform.setBasis(rotation_tf);
}
void homogeniousMatrixToTfTransform (const brics_3d::IHomogeneousMatrix44::IHomogeneousMatrix44Ptr& transformMatrix, tf::Transform& tfTransform) {
const double* matrixPtr = transformMatrix->getRawData();
btVector3 translation;
btMatrix3x3 rotation;
translation.setX(matrixPtr[12]);
translation.setY(matrixPtr[13]);
translation.setZ(matrixPtr[14]);
rotation.setValue(
matrixPtr[0], matrixPtr[4], matrixPtr[8],
matrixPtr[1], matrixPtr[5], matrixPtr[9],
matrixPtr[2], matrixPtr[6], matrixPtr[10]
);
tfTransform.setOrigin(translation);
tfTransform.setBasis(rotation);
}