SVGTransformDistance SVGTransformDistance::scaledDistance(float scaleFactor) const
{
switch (m_type) {
case SVGTransform::SVG_TRANSFORM_UNKNOWN:
return SVGTransformDistance();
case SVGTransform::SVG_TRANSFORM_ROTATE:
return SVGTransformDistance(m_type, m_angle * scaleFactor, m_cx * scaleFactor, m_cy * scaleFactor, AffineTransform());
case SVGTransform::SVG_TRANSFORM_SCALE:
case SVGTransform::SVG_TRANSFORM_MATRIX:
return SVGTransformDistance(m_type, m_angle * scaleFactor, m_cx * scaleFactor, m_cy * scaleFactor, AffineTransform(m_transform).scale(scaleFactor));
case SVGTransform::SVG_TRANSFORM_TRANSLATE:
{
AffineTransform newTransform(m_transform);
newTransform.setE(m_transform.e() * scaleFactor);
newTransform.setF(m_transform.f() * scaleFactor);
return SVGTransformDistance(m_type, 0, 0, 0, newTransform);
}
case SVGTransform::SVG_TRANSFORM_SKEWX:
case SVGTransform::SVG_TRANSFORM_SKEWY:
return SVGTransformDistance(m_type, m_angle * scaleFactor, m_cx * scaleFactor, m_cy * scaleFactor, AffineTransform());
}
ASSERT_NOT_REACHED();
return SVGTransformDistance();
}
tf::Transform SvdTransformOptimization::svdPCL(
std::vector<tf::Vector3> pointcloudX,
std::vector<tf::Vector3> pointcloudP) {
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;
pcl::PointCloud<pcl::PointXYZ> pclX, pclP;
pcl::Correspondences correspondences;
for (int i = 0; i < pointcloudX.size(); i++) {
pcl::PointXYZ pointX;
pointX.x = pointcloudX[i].getX();
pointX.y = pointcloudX[i].getY();
pointX.z = pointcloudX[i].getZ();
pclX.push_back(pointX);
pcl::PointXYZ pointP;
pointP.x = pointcloudP[i].getX();
pointP.y = pointcloudP[i].getY();
pointP.z = pointcloudP[i].getZ();
pclP.push_back(pointP);
correspondences.push_back(pcl::Correspondence(i, i, 0));
}
Eigen::Matrix4f tm;
svd.estimateRigidTransformation(pclX, pclP, correspondences, tm);
tf::Matrix3x3 Rtf(tm(0, 0), tm(0, 1), tm(0, 2), tm(1, 0), tm(1, 1),
tm(1, 2), tm(2, 0), tm(2, 1), tm(2, 2));
tf::Vector3 ttf(tm(0, 3), tm(1, 3), tm(2, 3));
tf::Transform newTransform(Rtf, ttf);
return newTransform;
}
bool CSTransform::Write2XML(TiXmlNode* root, bool parameterised, bool sparse)
{
UNUSED(sparse);
UNUSED(parameterised);
TiXmlElement Transform("Transformation");
for (size_t n=0;n<m_TransformList.size();++n)
{
TiXmlElement newTransform(GetNameByType(m_TransformList.at(n)).c_str());
std::string args;
for (size_t a=0;a<m_TransformArguments.at(n).size();++a)
{
args.append(m_TransformArguments.at(n).at(a).GetValueString());
if (a<m_TransformArguments.at(n).size()-1)
args.append(",");
}
newTransform.SetAttribute("Argument",args.c_str());
Transform.InsertEndChild(newTransform);
}
root->InsertEndChild(Transform);
return true;
}
SVGTransformDistance SVGTransformDistance::scaledDistance(
float scaleFactor) const {
switch (m_transformType) {
case kSvgTransformMatrix:
ASSERT_NOT_REACHED();
case kSvgTransformUnknown:
return SVGTransformDistance();
case kSvgTransformRotate:
return SVGTransformDistance(m_transformType, m_angle * scaleFactor,
m_cx * scaleFactor, m_cy * scaleFactor,
AffineTransform());
case kSvgTransformScale:
return SVGTransformDistance(
m_transformType, m_angle * scaleFactor, m_cx * scaleFactor,
m_cy * scaleFactor, AffineTransform(m_transform).scale(scaleFactor));
case kSvgTransformTranslate: {
AffineTransform newTransform(m_transform);
newTransform.setE(m_transform.e() * scaleFactor);
newTransform.setF(m_transform.f() * scaleFactor);
return SVGTransformDistance(m_transformType, 0, 0, 0, newTransform);
}
case kSvgTransformSkewx:
case kSvgTransformSkewy:
return SVGTransformDistance(m_transformType, m_angle * scaleFactor,
m_cx * scaleFactor, m_cy * scaleFactor,
AffineTransform());
}
ASSERT_NOT_REACHED();
return SVGTransformDistance();
}
tf::Transform SvdTransformOptimization::svdOwnImpl(
std::vector<tf::Vector3> pointcloudX,
std::vector<tf::Vector3> pointcloudP) {
// Calculate center of mass for both pointclouds.
int numOfPoints = pointcloudX.size();
tf::Vector3 centerOfMassX, centerOfMassP;
for (int i = 0; i < numOfPoints; i++) {
centerOfMassX += pointcloudX[i];
centerOfMassP += pointcloudP[i];
}
centerOfMassX /= numOfPoints;
centerOfMassP /= numOfPoints;
// Extract the center of mass from the corresponding points.
std::vector<tf::Vector3> pointcloudXPrime, pointcloudPPrime;
for (int i = 0; i < numOfPoints; i++) {
pointcloudXPrime.push_back(pointcloudX[i] - centerOfMassX);
pointcloudPPrime.push_back(pointcloudP[i] - centerOfMassP);
}
// Calculate matrix W
Eigen::MatrixXf W = Eigen::MatrixXf::Zero(3, 3);
for (int i = 0; i < numOfPoints; i++) {
Eigen::Vector3f currentPointXPrime(pointcloudXPrime[i].getX(),
pointcloudXPrime[i].getY(), pointcloudXPrime[i].getZ());
Eigen::Vector3f currentPointPPrime(pointcloudPPrime[i].getX(),
pointcloudPPrime[i].getY(), pointcloudPPrime[i].getZ());
W += currentPointXPrime * currentPointPPrime.transpose();
}
// Perform the SVD.
Eigen::JacobiSVD<Eigen::MatrixXf> svd(W);
svd.compute(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXf U = svd.matrixU();
Eigen::MatrixXf V = svd.matrixV();
// Caclulate Rotation and translation and convert to tf.
Eigen::MatrixXf R = U * V.transpose();
Eigen::Vector3f centerOfMassXEigen(centerOfMassX.getX(),
centerOfMassX.getY(), centerOfMassX.getZ());
Eigen::Vector3f centerOfMassPEigen(centerOfMassP.getX(),
centerOfMassP.getY(), centerOfMassP.getZ());
Eigen::MatrixXf t = centerOfMassXEigen - R * (centerOfMassPEigen);
tf::Matrix3x3 Rtf(R(0, 0), R(0, 1), R(0, 2), R(1, 0), R(1, 1), R(1, 2),
R(2, 0), R(2, 1), R(2, 2));
tf::Vector3 ttf(t(0), t(1), t(2));
// Create and return the new transform.
tf::Transform newTransform(Rtf, ttf);
return newTransform;
}
SVGTransform SVGTransformDistance::addToSVGTransform(const SVGTransform& transform) const
{
ASSERT(m_type == transform.type() || transform == SVGTransform());
SVGTransform newTransform(transform);
switch (m_type) {
case SVGTransform::SVG_TRANSFORM_UNKNOWN:
return SVGTransform();
case SVGTransform::SVG_TRANSFORM_MATRIX:
return SVGTransform(transform.matrix() * m_transform);
case SVGTransform::SVG_TRANSFORM_TRANSLATE:
{
FloatPoint translation = transform.translate();
translation += FloatSize::narrowPrecision(m_transform.e(), m_transform.f());
newTransform.setTranslate(translation.x(), translation.y());
return newTransform;
}
case SVGTransform::SVG_TRANSFORM_SCALE:
{
FloatSize scale = transform.scale();
scale += FloatSize::narrowPrecision(m_transform.a(), m_transform.d());
newTransform.setScale(scale.width(), scale.height());
return newTransform;
}
case SVGTransform::SVG_TRANSFORM_ROTATE:
{
// FIXME: I'm not certain the translation is calculated correctly here
FloatPoint center = transform.rotationCenter();
newTransform.setRotate(transform.angle() + m_angle,
center.x() + m_cx,
center.y() + m_cy);
return newTransform;
}
case SVGTransform::SVG_TRANSFORM_SKEWX:
newTransform.setSkewX(transform.angle() + m_angle);
return newTransform;
case SVGTransform::SVG_TRANSFORM_SKEWY:
newTransform.setSkewY(transform.angle() + m_angle);
return newTransform;
}
ASSERT_NOT_REACHED();
return SVGTransform();
}
void processTfTopic () {
map <string, vector<brics_3d::rsg::Attribute> >::iterator iter = objectClasses.begin();
for (iter = objectClasses.begin(); iter != objectClasses.end(); iter++) {
string objectFrameId = iter->first;
tf::StampedTransform transform;
try{
tfListener.lookupTransform(rootFrameId, objectFrameId, ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_WARN("%s",ex.what());
continue;
}
if ( (ros::Time::now() - transform.stamp_) > maxTFCacheDuration ) { //simply ignore outdated TF frames
ROS_WARN("TF found for %s. But it is outdated. Skipping it.", iter->first.c_str());
continue;
}
ROS_INFO("TF found for %s.", iter->first.c_str());
/* query */
vector<brics_3d::rsg::Attribute> queryAttributes;;
queryAttributes = iter->second;
vector<brics_3d::SceneObject> resultObjects;
myWM.getSceneObjects(queryAttributes, resultObjects);
// ROS_INFO("Number of boxes: %i " , static_cast<unsigned int>(resultObjects.size()));
/* associate */
unsigned int index = -1;
double minSquardDistanceToExistingObjects = std::numeric_limits<double>::max();
const double* matrixPtr;
for (unsigned int i = 0; i < static_cast<unsigned int>(resultObjects.size()) ; ++i) {
matrixPtr = resultObjects[i].transform->getRawData();
double squardDistanceToExistingObjects;
double xPercieved = transform.getOrigin().x();
double yPercieved = transform.getOrigin().y();
double zPercieved = transform.getOrigin().z();
double xStored = matrixPtr[12];
double yStored = matrixPtr[13];
double zStored = matrixPtr[14];
squardDistanceToExistingObjects = (xPercieved - xStored) * (xPercieved - xStored) +
(yPercieved - yStored) * (yPercieved - yStored) +
(zPercieved - zStored) * (zPercieved - zStored);
if (squardDistanceToExistingObjects < minSquardDistanceToExistingObjects) {
minSquardDistanceToExistingObjects = squardDistanceToExistingObjects;
index = i;
}
}
ROS_INFO("Shortest distance %lf to found result object %i.", minSquardDistanceToExistingObjects, index);
if (minSquardDistanceToExistingObjects < (associationDistanceTreshold * associationDistanceTreshold) ) {
/* update existing */
ROS_INFO("Updating existing scene object with object ID: %i", resultObjects[index].id);
brics_3d::IHomogeneousMatrix44::IHomogeneousMatrix44Ptr newTransform(new brics_3d::HomogeneousMatrix44());
tfTransformToHomogeniousMatrix(transform, newTransform);
myWM.insertTransform(resultObjects[index].id, newTransform);
} else {
/* insert */
ROS_INFO("Inserting new scene object");
brics_3d::rsg::Shape::ShapePtr boxShape(new brics_3d::rsg::Box(cubeSize, cubeSize, cubeSize)); // in [m]
brics_3d::rsg::Shape::ShapePtr targetAreaBoxShape(new brics_3d::rsg::Box(targetAreaSizeX, targetAreaSizeY, targetAreaSizeZ)); // in [m]
brics_3d::IHomogeneousMatrix44::IHomogeneousMatrix44Ptr initialTransform(new brics_3d::HomogeneousMatrix44());
tfTransformToHomogeniousMatrix(transform, initialTransform);
brics_3d::SceneObject tmpSceneObject;
if ( (iter->first.compare(startFrameId) == 0) || (iter->first.compare(auxiliaryFrameId) == 0) || (iter->first.compare(goalFrameId) == 0)) {
tmpSceneObject.shape = targetAreaBoxShape;
} else {
tmpSceneObject.shape = boxShape;
}
// tmpSceneObject.shape = boxShape;
tmpSceneObject.transform = initialTransform;
tmpSceneObject.parentId = myWM.getRootNodeId(); // hook in after root node
tmpSceneObject.attributes.clear();
tmpSceneObject.attributes = iter->second;
unsigned int returnedId;
myWM.addSceneObject(tmpSceneObject, returnedId);
}
}
/* query */
vector<brics_3d::rsg::Attribute> queryAttributes;
vector<brics_3d::SceneObject> resultObjects;
queryAttributes.push_back(brics_3d::rsg::Attribute("shapeType","Box"));
// queryAttributes.push_back(brics_3d::rsg::Attribute("color","red"));
myWM.getSceneObjects(queryAttributes, resultObjects);
ROS_INFO("Total number of boxes: %i " , static_cast<unsigned int>(resultObjects.size()));
}
