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())); }