C++ Quaterniond::inverse方法代码示例

/**
 * Recursion method to be used with traverseTreeTopDown() and recursion parameters
 * of type *Vector3RecursionParams*.
 *
 * Re-arranges the joint-transform of the recursion link's *parent joint*, along with
 * the link's visual/collision/intertial rotations, such that all joints rotate around the axis
 * given in the recursion parameters vector.
 */
int allRotationsToAxisCB(urdf_traverser::RecursionParamsPtr& p)
{
    urdf_traverser::LinkPtr link = p->getLink();
    if (!link)
    {
        ROS_ERROR("allRotationsToAxis: NULL link passed");
        return -1;
    }

    Vector3RecursionParams::Ptr param = baselib_binding_ns::dynamic_pointer_cast<Vector3RecursionParams>(p);
    if (!param)
    {
        ROS_ERROR("Wrong recursion parameter type");
        return -1;
    }

    urdf_traverser::JointPtr joint = link->parent_joint;
    if (!joint)
    {
        ROS_INFO_STREAM("allRotationsToAxis: Joint for link " << link->name << " is NULL, so this must be the root joint");
        return 1;
    }

    Eigen::Vector3d axis = param->vec;

    Eigen::Quaterniond alignAxis;
    if (urdf_traverser::jointTransformForAxis(joint, axis, alignAxis))
    {
        Eigen::Vector3d rotAxis(joint->axis.x, joint->axis.y, joint->axis.z);
        // ROS_INFO_STREAM("Transforming axis "<<rotAxis<<" for joint "<<joint->name<<" with transform "<<urdf_traverser::EigenTransform(alignAxis));

        urdf_traverser::applyTransform(joint, urdf_traverser::EigenTransform(alignAxis), false);

        // the link has to receive the inverse transform, so it stays at the original position
        Eigen::Quaterniond alignAxisInv = alignAxis.inverse();
        urdf_traverser::applyTransform(link, urdf_traverser::EigenTransform(alignAxisInv), true);

        // we also have to fix the child joint's (1st order child joints) transform
        // to correct for this transformation.
        for (std::vector<urdf_traverser::JointPtr>::iterator pj = link->child_joints.begin();
                pj != link->child_joints.end(); pj++)
        {
            urdf_traverser::applyTransform(*pj, urdf_traverser::EigenTransform(alignAxisInv), true);
        }

        // finally, set the rotation axis to the target
        joint->axis.x = axis.x();
        joint->axis.y = axis.y();
        joint->axis.z = axis.z();
    }

    // all good, indicate that recursion can continue
    return 1;
}

void DeformToFit::registerAndDeformNodes(Structure::Node * snode, Structure::Node * tnode)
{
    auto scenter = snode->position(Eigen::Vector4d(0.5, 0.5, 0, 0));
    auto tcenter = tnode->position(Eigen::Vector4d(0.5, 0.5, 0, 0));

    auto translation = Vector3(tcenter - scenter);

	if (snode->type() == tnode->type())
	{
		if (snode->type() == Structure::CURVE)
		{
			auto scpts = snode->controlPoints();
			auto tcpts = tnode->controlPoints();

			for (auto & p : tcpts) p -= translation;

			// Register
			Vector3 sfront = scpts.front();
			Vector3 tfront = tcpts.front();
			Vector3 tback = tcpts.back();
			bool isReverse = (sfront - tfront).norm() > (sfront - tback).norm() ? true : false;
			if (isReverse) std::reverse(tcpts.begin(), tcpts.end());

			// Encode target curve as deltas from center
			std::map < double, Vector3 > deltas;
			for (size_t i = 0; i < tcpts.size(); i++){
				double t = double(i) / (tcpts.size() - 1);
				deltas[t] = tcpts[i] - scenter;
			}

			// Deform source curve from deltas
			for (size_t i = 0; i < scpts.size(); i++){
				double t = double(i) / (scpts.size() - 1);
				scpts[i] = (scenter + translation) + linear_interpolate<Vector3>(t, deltas);
			}

			// Apply deformed control points
			snode->setControlPoints(scpts);
		}

		if (snode->type() == Structure::SHEET)
		{
			Structure::Sheet * ssheet = (Structure::Sheet*) snode;
			Structure::Sheet * tsheet = (Structure::Sheet*) tnode;

			auto ssurface = ssheet->surface;
			auto tsurface = tsheet->surface;

			// Remove translation
			tsurface.translate(-translation);
			
			// Minimize rotation
			Vector3 tpos, tu, tv, tnormal;
			tsurface.GetFrame(0.5, 0.5, tpos, tu, tv, tnormal);
			Vector3 spos, su, sv, snormal;
			ssurface.GetFrame(0.5, 0.5, spos, su, sv, snormal);

			if (snormal.dot(tnormal) < 0) 
				tnormal *= -1;

			Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(snormal, tnormal);
			for (auto & row : tsurface.mCtrlPoint) for (auto & p : row) p = (q.inverse() * (p - scenter)) + scenter;

			QMap < double, QVector<double> > dists;
			for (double u = 0; u <= 1.0; u += 1.0){
				for (double v = 0; v <= 1.0; v += 1.0){
					for (double i = 0; i <= 1.0; i += 1.0){
						for (double j = 0; j <= 1.0; j += 1.0){
							dists[(ssurface.P(u, v) - tsurface.P(i, j)).norm()] = (QVector<double>() << u << v << i << j);
						}
					}
				}
			}

			auto bestChoice = dists.values().front();
			bool isReverseU = bestChoice[0] != bestChoice[2], isReverseV = bestChoice[1] != bestChoice[3];

			// Reverse if needed
			if ( isReverseV ){
				for (int i = 0; i < (int)tsurface.mCtrlPoint.size(); i++){
					std::reverse(tsurface.mCtrlPoint[i].begin(), tsurface.mCtrlPoint[i].end());
					std::reverse(tsurface.mCtrlWeight[i].begin(), tsurface.mCtrlWeight[i].end());
				}
			}
			if( isReverseU ){
				std::reverse(tsurface.mCtrlPoint.begin(), tsurface.mCtrlPoint.end());
				std::reverse(tsurface.mCtrlWeight.begin(), tsurface.mCtrlWeight.end());
			}

			std::map < double, size_t > mapU, mapV;
			for (size_t i = 0; i < tsurface.mNumUCtrlPoints; i++) mapU[double(i) / (tsurface.mNumUCtrlPoints - 1)] = i;
			for (size_t j = 0; j < tsurface.mNumVCtrlPoints; j++) mapV[double(j) / (tsurface.mNumVCtrlPoints - 1)] = j;

			auto getQuad = [&](size_t u, size_t v, Array2D_Vector3& cpts){ 
				return QVector<Vector3>() << cpts[u][v] << cpts[u+1][v] << cpts[u][v+1] << cpts[u+1][v+1];
			};

			for (size_t i = 0; i < ssurface.mNumUCtrlPoints; i++){
				for (size_t j = 0; j < ssurface.mNumVCtrlPoints; j++){
					double u = double(i) / (ssurface.mNumUCtrlPoints-1);
//.........这里部分代码省略.........

 /**
  * @brief Rotate the map relative to its local frame
  * 
  * @param rotation <a href="http://eigen.tuxfamily.org/dox/group__Geometry__Module.html#ga0d2bd45f1215359f8e7c0d7ab53c4acb" target="_blank">
  * Eigen::Quaterniond</a>
  */
 void rotate(const Eigen::Quaterniond &rotation)
 {
     data_ptr->offset.rotate(rotation.inverse());
 }            

Eigen::Quaterniond UAS::transform_frame(const Eigen::Quaterniond &q)
{
	return FRAME_ROTATE_Q * q * FRAME_ROTATE_Q.inverse();
}