C++ Matrix4f::block方法代码示例

void PairwiseRegistrationDialog::showResults(const Eigen::Matrix4f &transformation, float rmsError, int corrNumber)
{
	std::stringstream temp;
	Eigen::Matrix4f T = transformation;
	temp.str("");
	temp << T;
	TTextEdit->setText(QString::fromStdString(temp.str()));
	Eigen::Matrix3f R = T.block(0,0,3,3);
	float c = (R * R.transpose()).diagonal().mean();
	c = qSqrt(c);
	cLineEdit->setText(QString::number(c));
	R /= c;
	temp.str("");
	temp << R;
	RTextEdit->setText(QString::fromStdString(temp.str()));
	Eigen::Vector3f t = T.block(0,3,3,1);
	temp.str("");
	temp << t;
	tLineEdit->setText(QString::fromStdString(temp.str()));
	Eigen::AngleAxisf angleAxis(R);
	angleLineEdit->setText( QString::number(angleAxis.angle()) );
	temp.str("");
	temp << angleAxis.axis();
	axisLineEdit->setText(QString::fromStdString(temp.str()));

	corrNumberLineEdit->setText(QString::number(corrNumber));
	errorLineEdit->setText(QString::number(rmsError));
}

SimoxMotionState::SimoxMotionState( VirtualRobot::SceneObjectPtr sceneObject)
    :btDefaultMotionState()
{
	this->sceneObject = sceneObject;
	initalGlobalPose.setIdentity();
	if (sceneObject)
	{
		initalGlobalPose = sceneObject->getGlobalPoseVisualization();
	}
    _transform.setIdentity();
	_graphicsTransfrom.setIdentity();
	_comOffset.setIdentity();
	Eigen::Vector3f com = sceneObject->getCoMLocal();
	RobotNodePtr rn = boost::dynamic_pointer_cast<RobotNode>(sceneObject);
	if (rn)
	{
		// we are operating on a RobotNode, so we need a RobtoNodeActuator to move it later on
		robotNodeActuator.reset(new RobotNodeActuator(rn));

		// localcom is given in coord sytsem of robotnode (== endpoint of internal transformation)
		// we need com in visualization coord system (== without postjointtransform)

		Eigen::Matrix4f t;
		t.setIdentity();
		t.block(0,3,3,1)=rn->getCoMGlobal();
		t = rn->getGlobalPoseVisualization().inverse() * t;
		com = t.block(0,3,3,1);
	}
	_setCOM(com);
	setGlobalPose(initalGlobalPose);
}

void stabilityWindow::updateCoM()
{
    if (!comVisu || comVisu->getNumChildren() == 0)
    {
        return;
    }

    // Draw CoM
    Eigen::Matrix4f globalPoseCoM;
    globalPoseCoM.setIdentity();

    if (currentRobotNodeSet)
    {
        globalPoseCoM.block(0, 3, 3, 1) = currentRobotNodeSet->getCoM();
    }
    else if (robot)
    {
        globalPoseCoM.block(0, 3, 3, 1) = robot->getCoMGlobal();
    }

    SoMatrixTransform* m = dynamic_cast<SoMatrixTransform*>(comVisu->getChild(0));

    if (m)
    {
        SbMatrix ma(reinterpret_cast<SbMat*>(globalPoseCoM.data()));
        // mm -> m
        ma[3][0] *= 0.001f;
        ma[3][1] *= 0.001f;
        ma[3][2] *= 0.001f;
        m->matrix.setValue(ma);
    }

    // Draw CoM projection
    if (currentRobotNodeSet && comProjectionVisu && comProjectionVisu->getNumChildren() > 0)
    {
        globalPoseCoM(2, 3) = 0;
        m = dynamic_cast<SoMatrixTransform*>(comProjectionVisu->getChild(0));

        if (m)
        {
            SbMatrix ma(reinterpret_cast<SbMat*>(globalPoseCoM.data()));
            // mm -> m
            ma[3][0] *= 0.001f;
            ma[3][1] *= 0.001f;
            ma[3][2] *= 0.001f;
            m->matrix.setValue(ma);
        }
    }
}

Eigen::Matrix4f DynamicsRobot::getComGlobal( VirtualRobot::RobotNodePtr rn )
{
    Eigen::Matrix4f com = Eigen::Matrix4f::Identity();
    com.block(0,3,3,1) = rn->getCoMLocal();
    com = rn->getGlobalPoseVisualization()*com;
    return com;
}

void GraphOptimizer_G2O::addEdge(const int fromIdx,
                                 const int toIdx,
                                 Eigen::Matrix4f& relativePose,
                                 Eigen::Matrix<double,6,6>& informationMatrix)
{
    #if ENABLE_DEBUG_G2O
    char* fileName = (char*) malloc(100);
    std::sprintf(fileName,"../results/matrices/edge_%i_to_%i.txt",fromIdx,toIdx);
    Miscellaneous::saveMatrix(relativePose,fileName);
    delete fileName;
    #endif

    //Transform Eigen::Matrix4f into 3D traslation and rotation for g2o
    g2o::Vector3d t(relativePose(0,3),relativePose(1,3),relativePose(2,3));
    g2o::Quaterniond q;
    Eigen::Matrix3d rot = relativePose.block(0,0,3,3).cast<double>();
    q = rot;


    g2o::SE3Quat transf(q,t);	// relative transformation

    g2o::EdgeSE3* edge = new g2o::EdgeSE3;
    edge->vertices()[0] = optimizer.vertex(fromIdx);
    edge->vertices()[1] = optimizer.vertex(toIdx);
    edge->setMeasurement(transf);
    edge->setInformation(informationMatrix);

    optimizer.addEdge(edge);
}

void stabilityWindow::comTargetMovedX(int value)
{
	if(!currentRobotNodeSet)
		return;

	Eigen::Matrix4f T;
	T.setIdentity();

	m_CoMTarget(0) = value;
	T.block(0, 3, 2, 1) = m_CoMTarget;

	if(comTargetVisu && comTargetVisu->getNumChildren() > 0)
	{
		SoMatrixTransform *m = dynamic_cast<SoMatrixTransform *>(comTargetVisu->getChild(0));
		if(m)
		{
			SbMatrix ma(reinterpret_cast<SbMat*>(T.data()));
            // mm -> m
            ma[3][0] *= 0.001f;
            ma[3][1] *= 0.001f;
            ma[3][2] *= 0.001f;
			m->matrix.setValue(ma);
		}
	}

	performCoMIK();
}

void SimoxMotionState::_setCOM( const Eigen::Vector3f& com )
{
	this->com = com;
	Eigen::Matrix4f comM = Eigen::Matrix4f::Identity();
	comM.block(0,3,3,1) = -com;
	_comOffset = BulletEngine::getPoseBullet(comM);
}

Eigen::Matrix4f getInverseCameraMatrix( Eigen::Matrix4f &cameraPose )
{
    auto rotation = cameraPose.block(0, 0, 3, 3);
    auto translation = cameraPose.block(0, 3, 4, 1);

    auto rotationInv = (Eigen::Matrix4f)rotation.inverse();
    auto translationInv = -rotationInv * translation;

    Eigen::Matrix4f cameraPoseInv;
    cameraPoseInv << rotationInv(0,0) ,rotationInv(0,1) ,rotationInv(0,2)   ,translationInv(0)
            ,rotationInv(1,0) ,rotationInv(1,1) ,rotationInv(1,2)   ,translationInv(1)
            ,rotationInv(2,0) ,rotationInv(2,1) ,rotationInv(2,2)   ,translationInv(2)
            ,0                ,0                ,0                  ,1 ;

    return cameraPoseInv;
}

 bool ApproachMovementGenerator::updateEEFPose(const Eigen::Vector3f& deltaPosition)
 {
     Eigen::Matrix4f deltaPose;
     deltaPose.setIdentity();
     deltaPose.block(0, 3, 3, 1) = deltaPosition;
     return updateEEFPose(deltaPose);
 }

bool GraspPlannerEvaluatorWindow::updateEEFPose(const Eigen::Vector3f& deltaPosition)
{
	Eigen::Matrix4f deltaPose;
	deltaPose.setIdentity();
	deltaPose.block(0, 3, 3, 1) = deltaPosition;
	return updateEEFPose(deltaPose);
}

void GraspPlannerEvaluatorWindow::pertubateStepObject()
{
	Eigen::Vector3f rotPertub;
	rotPertub.setRandom(3).normalize();
	Eigen::Vector3f translPertub;
	translPertub.setRandom(3).normalize();

	Eigen::Matrix4f deltaPose;
	deltaPose.setIdentity();

        translPertub(0) =  UI.doubleSpinBoxPertDistanceX->value();
        translPertub(1) =  UI.doubleSpinBoxPertDistanceY->value();
        translPertub(2) =  UI.doubleSpinBoxPertDistanceZ->value();

        // rotPertub(0) =  UI.doubleSpinBoxPertAngleX->value();
        // rotPertub(1) =  UI.doubleSpinBoxPertAngleY->value();
        /* rotPertub(2) =  UI.doubleSpinBoxPertAngleZ->value(); */

        
        //deltaPose.block(0,0,3,3) = rodriguesFormula(rotPertub, UI.doubleSpinBoxPertAngle->value());
	// deltaPose.block(0,0,3,3) = rotPertub;
        deltaPose.block(0,3,3,1) = translPertub;

	std::cout << "DeltaPose:\n" << deltaPose << std::endl;
	std::cout << "Pose:\n" << object->getGlobalPose() << std::endl;
	std::cout << "FinalPose:\n" << (deltaPose*object->getGlobalPose()) << std::endl;
	object->setGlobalPose(deltaPose*object->getGlobalPose());
}

void SimDynamicsWindow::updateComVisu()
{
    if (!robot)
    {
        return;
    }

    std::vector<RobotNodePtr> n = robot->getRobotNodes();
    std::map< VirtualRobot::RobotNodePtr, SoSeparator* >::iterator i = comVisuMap.begin();

    while (i != comVisuMap.end())
    {
        SoSeparator* sep = i->second;
        SoMatrixTransform* m = dynamic_cast<SoMatrixTransform*>(sep->getChild(0));

        if (m)
        {
            Eigen::Matrix4f ma = dynamicsRobot->getComGlobal(i->first);
            ma.block(0, 3, 3, 1) *= 0.001f;
            m->matrix.setValue(CoinVisualizationFactory::getSbMatrix(ma));
        }

        i++;
    }
}

    bool isOdometryContinuousMotion(Eigen::Matrix4f &prevPose, Eigen::Matrix4f &currPose, float thresDist = 0.1)
    {
        Eigen::Matrix4f relativePose = prevPose.inverse() * currPose;
        if( relativePose.block(0,3,3,1).norm() > thresDist )
            return false;

        return true;
    }

/**
 * estimateRigidTransformationSVD
 */
void RigidTransformationRANSAC::estimateRigidTransformationSVD(
      const std::vector<Eigen::Vector3f > &srcPts,
      const std::vector<int> &srcIndices,
      const std::vector<Eigen::Vector3f > &tgtPts,
      const std::vector<int> &tgtIndices,
      Eigen::Matrix4f &transform)
{
  Eigen::Vector3f srcCentroid, tgtCentroid;

  // compute the centroids of source, target
  ComputeCentroid (srcPts, srcIndices, srcCentroid);
  ComputeCentroid (tgtPts, tgtIndices, tgtCentroid);

  // Subtract the centroids from source, target
  Eigen::MatrixXf srcPtsDemean;
  DemeanPoints(srcPts, srcIndices, srcCentroid, srcPtsDemean);

  Eigen::MatrixXf tgtPtsDemean;
  DemeanPoints(tgtPts, tgtIndices, tgtCentroid, tgtPtsDemean);

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix3f H = (srcPtsDemean * tgtPtsDemean.transpose ()).topLeftCorner<3, 3>();

  // Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3f u = svd.matrixU ();
  Eigen::Matrix3f v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  // Return the transformation
  Eigen::Matrix3f R = v * u.transpose ();
  Eigen::Vector3f t = tgtCentroid - R * srcCentroid;

  // set rotation
  transform.block(0,0,3,3) = R;
  // set translation
  transform.block(0,3,3,1) = t;
  transform.block(3, 0, 1, 3).setZero();  
  transform(3,3) = 1.;
}

void HierarchicalWalkingIK::computeWalkingTrajectory(const Eigen::Matrix3Xf& comTrajectory,
                                                     const Eigen::Matrix6Xf& rightFootTrajectory,
                                                     const Eigen::Matrix6Xf& leftFootTrajectory,
                                                     std::vector<Eigen::Matrix3f>& rootOrientation,
                                                     Eigen::MatrixXf& trajectory)
{
    int rows = outputNodeSet->getSize();

    trajectory.resize(rows, rightFootTrajectory.cols());
    rootOrientation.resize(rightFootTrajectory.cols());

    Eigen::Vector3f com = colModelNodeSet->getCoM();

    Eigen::VectorXf configuration;
    int N = trajectory.cols();

    Eigen::Matrix4f leftFootPose  = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f rightFootPose = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f chestPose     = chest->getGlobalPose();
    Eigen::Matrix4f pelvisPose    = pelvis->getGlobalPose();

    for (int i = 0; i < N; i++)
    {
        VirtualRobot::MathTools::posrpy2eigen4f(1000 * leftFootTrajectory.block(0, i, 3, 1),  leftFootTrajectory.block(3, i, 3, 1),  leftFootPose);
        VirtualRobot::MathTools::posrpy2eigen4f(1000 * rightFootTrajectory.block(0, i, 3, 1), rightFootTrajectory.block(3, i, 3, 1), rightFootPose);

        // FIXME the orientation of the chest and chest is specific to armar 4
        // since the x-Axsis points in walking direction
        Eigen::Vector3f xAxisChest = (leftFootPose.block(0, 1, 3, 1) + rightFootPose.block(0, 1, 3, 1))/2;
        xAxisChest.normalize();
        chestPose.block(0, 0, 3, 3) = Bipedal::poseFromXAxis(xAxisChest);
        pelvisPose.block(0, 0, 3, 3) = Bipedal::poseFromYAxis(-xAxisChest);

        std::cout << "Frame #" << i << ", ";
        robot->setGlobalPose(leftFootPose);
        computeStepConfiguration(1000 * comTrajectory.col(i),
                                 rightFootPose,
                                 chestPose,
                                 pelvisPose,
                                 configuration);

        trajectory.col(i) = configuration;
        rootOrientation[i] = leftFootPose.block(0, 0, 3, 3);
    }
}