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));
}
Esempio n. 2
0
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);
}
Esempio n. 3
0
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);
        }
    }
}
Esempio n. 4
0
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);
}
Esempio n. 6
0
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();
}
Esempio n. 7
0
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);
}
Esempio n. 8
0
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;
}
Esempio n. 9
0
 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());
}
Esempio n. 12
0
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++;
    }
}
Esempio n. 13
0
    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;
    }
Esempio n. 14
0
/**
 * 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);
    }
}
Eigen::Matrix4f CUDACameraTrackingMultiResRGBD::computeBestRigidAlignment(CameraTrackingInput cameraTrackingInput, Eigen::Matrix3f& intrinsics, Eigen::Matrix4f& globalDeltaTransform, unsigned int level, CameraTrackingParameters cameraTrackingParameters, unsigned int maxInnerIter, float condThres, float angleThres, LinearSystemConfidence& conf) 
{
	Eigen::Matrix4f deltaTransform = globalDeltaTransform;

	conf.reset();

	Matrix6x7f system;

	Eigen::Matrix3f ROld = deltaTransform.block(0, 0, 3, 3);
	Eigen::Vector3f eulerAngles = ROld.eulerAngles(2, 1, 0);

	float3 anglesOld;	   anglesOld.x = eulerAngles.x(); anglesOld.y = eulerAngles.y(); anglesOld.z = eulerAngles.z();
	float3 translationOld; translationOld.x = deltaTransform(0, 3); translationOld.y = deltaTransform(1, 3); translationOld.z = deltaTransform(2, 3);

	m_CUDABuildLinearSystem->applyBL(cameraTrackingInput, intrinsics, cameraTrackingParameters, anglesOld, translationOld, m_imageWidth[level], m_imageHeight[level], level, system, conf);

	Matrix6x6f ATA = system.block(0, 0, 6, 6);
	Vector6f ATb = system.block(0, 6, 6, 1);

	if (ATA.isZero()) {
		return m_matrixTrackingLost;
	}

	Eigen::JacobiSVD<Matrix6x6f> SVD(ATA, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Vector6f x = SVD.solve(ATb);

	//computing the matrix condition
	Vector6f evs = SVD.singularValues();
	conf.matrixCondition = evs[0]/evs[5];

	Vector6f xNew; xNew.block(0, 0, 3, 1) = eulerAngles; xNew.block(3, 0, 3, 1) = deltaTransform.block(0, 3, 3, 1);
	xNew += x;

	deltaTransform = delinearizeTransformation(xNew, Eigen::Vector3f(0.0f, 0.0f, 0.0f), 1.0f, level);
	if(deltaTransform(0, 0) == -std::numeric_limits<float>::infinity())
	{
		conf.trackingLostTresh = true;
		return m_matrixTrackingLost;
	}

	return deltaTransform;
}
Esempio n. 17
0
void SimoxMotionState::setGlobalPose( const Eigen::Matrix4f &pose )
{
    initalGlobalPose = pose;
	/* convert to local coord system, apply comoffset and convert back*/
	Eigen::Matrix4f poseLocal = sceneObject->getGlobalPoseVisualization().inverse() * pose;
	poseLocal.block(0,3,3,1) += com;
	Eigen::Matrix4f poseGlobal = sceneObject->getGlobalPoseVisualization() * poseLocal;
	m_startWorldTrans = BulletEngine::getPoseBullet(poseGlobal);
	//m_startWorldTrans.getOrigin() -= _comOffset.getOrigin();
    updateTransform();  
}
Esempio n. 18
0
Eigen::Matrix4f ConsistencyTest::initPose2D( std::map<unsigned, unsigned> &matched_planes )
{
  //Calculate rotation
  Matrix3f normalCovariances = Matrix3f::Zero();
  for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
    normalCovariances += PBMTarget.vPlanes[it->second].v3normal * PBMSource.vPlanes[it->first].v3normal.transpose();
  normalCovariances(1,1) += 100; // Rotation "restricted" to the y axis

  JacobiSVD<MatrixXf> svd(normalCovariances, ComputeThinU | ComputeThinV);
  Matrix3f Rotation = svd.matrixU() * svd.matrixV().transpose();

  if(Rotation.determinant() < 0)
//    Rotation.row(2) *= -1;
    Rotation = -Rotation;

  // Calculate translation
  Vector3f translation;
  Vector3f center_data = Vector3f::Zero(), center_model = Vector3f::Zero();
  Vector3f centerFull_data = Vector3f::Zero(), centerFull_model = Vector3f::Zero();
  unsigned numFull = 0, numNonStruct = 0;
  for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
  {
    if(PBMSource.vPlanes[it->first].bFromStructure) // The certainty in center of structural planes is too low
      continue;

    ++numNonStruct;
    center_data += PBMSource.vPlanes[it->first].v3center;
    center_model += PBMTarget.vPlanes[it->second].v3center;
    if(PBMSource.vPlanes[it->first].bFullExtent)
    {
      centerFull_data += PBMSource.vPlanes[it->first].v3center;
      centerFull_model += PBMTarget.vPlanes[it->second].v3center;
      ++numFull;
    }
  }
  if(numFull > 0)
  {
    translation = (-centerFull_model + Rotation * centerFull_data) / numFull;
  }
  else
  {
    translation = (-center_model + Rotation * center_data) / numNonStruct;
  }

  translation[1] = 0; // Restrict no translation in the y axis

  // Form SE3 transformation matrix. This matrix maps the model into the current data reference frame
  Eigen::Matrix4f rigidTransf;
  rigidTransf.block(0,0,3,3) = Rotation;
  rigidTransf.block(0,3,3,1) = translation;
  rigidTransf.row(3) << 0,0,0,1;
  return rigidTransf;
}
void PlayWindow::ShowPC(PointCloudT::Ptr PC)
{
    updateModelMutex.lock();


    if (ui->checkBox_ShowPC->isChecked())
    {

        if (!viewer->updatePointCloud(PC))
            viewer->addPointCloud(PC);

        Eigen::Matrix4f currentPose = Eigen::Matrix4f::Identity();
        currentPose.block(0,0,3,3) = PC->sensor_orientation_.matrix();
        currentPose.block(0,3,3,1) = PC->sensor_origin_.head(3);

        viewer->updatePointCloudPose("cloud",Eigen::Affine3f(currentPose) );

    }



    ui->qvtkWidget->update ();


    updateModelMutex.unlock();



    // Timing
    times.push_back(QDateTime::currentDateTime().toMSecsSinceEpoch() - PC->header.stamp);

    double mean;
    if (times.size() > 60)
    {
        mean = std::accumulate(times.begin(), times.end(), 0.0) / times.size();
        times.erase(times.begin());
    }

    if (ui->checkBox_SavePC->isChecked())
        pcl::io::savePCDFileASCII(PC->header.frame_id, *PC);


    std::vector<int> ind;
    pcl::removeNaNFromPointCloud(*PC, *PC, ind);
    ui->label_nPoint->setText(QString("Nb Point : %1").arg(PC->size()));
    ui->label_time->setText(QString("Time (ms) : %1").arg(mean));
    ui->label_fps->setText(QString("FPS (Hz) : %1").arg(1000/mean));



    return;
}
Esempio n. 20
0
    std::vector< Eigen::Vector3f > CollisionModel::getModelVeticesGlobal()
    {
        std::vector< Eigen::Vector3f > result;
        TriMeshModelPtr model = collisionModelImplementation->getTriMeshModel();

        if (!model)
        {
            return result;
        }

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

        for (std::vector<Eigen::Vector3f >::iterator i = model->vertices.begin(); i != model->vertices.end(); i++)
        {
            t.block(0, 3, 3, 1) = *i;
            t = globalPose * t;
            result.push_back(t.block(0, 3, 3, 1));
        }

        return result;
    }
/*void CameraPoseFinderICP::findCorresSet(unsigned level,const Mat44& cur_transform,const Mat44& last_transform_inv)
{
	 cudaProjectionMapFindCorrs(level,cur_transform,last_transform_inv,
			 _camera_params_pyramid[level],
			AppParams::instance()->_icp_params.fDistThres,
			AppParams::instance()->_icp_params.fNormSinThres);

}*/
bool CameraPoseFinderICP::vector6ToTransformMatrix(const Eigen::Matrix<float, 6, 1, 0, 6, 1>& x,  Eigen::Matrix4f& output)
{
	Eigen::Matrix3f R = Eigen::AngleAxisf(x[0], Eigen::Vector3f::UnitX()).toRotationMatrix()
		*Eigen::AngleAxisf(x[1], Eigen::Vector3f::UnitY()).toRotationMatrix()
		*Eigen::AngleAxisf(x[2], Eigen::Vector3f::UnitZ()).toRotationMatrix();
	Eigen::Vector3f t = x.segment(3, 3);
	Eigen::AngleAxisf aa(R);
	float angle = aa.angle();
	float d = t.norm();
	if (angle > AppParams::instance()->_icp_params.fAngleShake|| d> AppParams::instance()->_icp_params.fDistShake)
	{
		return false;
	}
	output.block(0, 0, 3, 3) = R;
	output.block(0, 3, 3, 1) = t;
	return true;
}
Esempio n. 22
0
void SQ_fitter<PointT>::getBoundingBox(const PointCloudPtr &_cloud,
				       double _dim[3],
				       double _trans[3],
				       double _rot[3] ) {

  // 1. Compute the bounding box center
  Eigen::Vector4d centroid;
  pcl::compute3DCentroid( *_cloud, centroid );
  _trans[0] = centroid(0);
  _trans[1] = centroid(1); 
  _trans[2] = centroid(2);

  // 2. Compute main axis orientations
  pcl::PCA<PointT> pca;
  pca.setInputCloud( _cloud );
  Eigen::Vector3f eigVal = pca.getEigenValues();
  Eigen::Matrix3f eigVec = pca.getEigenVectors();
  // Make sure 3 vectors are normal w.r.t. each other
  Eigen::Vector3f temp;
  eigVec.col(2) = eigVec.col(0); // Z
  Eigen::Vector3f v3 = (eigVec.col(1)).cross( eigVec.col(2) );
  eigVec.col(0) = v3;
  Eigen::Vector3f rpy = eigVec.eulerAngles(2,1,0);
 
  _rot[0] = (double)rpy(2);
  _rot[1] = (double)rpy(1);
  _rot[2] = (double)rpy(0);

  // Transform _cloud
  Eigen::Matrix4f transf = Eigen::Matrix4f::Identity();
  transf.block(0,3,3,1) << (float)centroid(0), (float)centroid(1), (float)centroid(2);
  transf.block(0,0,3,3) = eigVec;

  Eigen::Matrix4f tinv; tinv = transf.inverse();
  PointCloudPtr cloud_temp( new pcl::PointCloud<PointT>() );
  pcl::transformPointCloud( *_cloud, *cloud_temp, tinv );

  // Get maximum and minimum
  PointT minPt; PointT maxPt;
  pcl::getMinMax3D( *cloud_temp, minPt, maxPt );
  
  _dim[0] = ( maxPt.x - minPt.x ) / 2.0;
  _dim[1] = ( maxPt.y - minPt.y ) / 2.0;
  _dim[2] = ( maxPt.z - minPt.z ) / 2.0;

}
Eigen::Matrix4f CUDACameraTrackingMultiResRGBD::delinearizeTransformation(Vector6f& x, Eigen::Vector3f& mean, float meanStDev, unsigned int level) 
{
	Eigen::Matrix3f R =	 Eigen::AngleAxisf(x[0], Eigen::Vector3f::UnitZ()).toRotationMatrix()  // Rot Z
						*Eigen::AngleAxisf(x[1], Eigen::Vector3f::UnitY()).toRotationMatrix()  // Rot Y
						*Eigen::AngleAxisf(x[2], Eigen::Vector3f::UnitX()).toRotationMatrix(); // Rot X

	Eigen::Vector3f t = x.segment(3, 3);

	if(!checkRigidTransformation(R, t, GlobalCameraTrackingState::getInstance().s_angleTransThres[level], GlobalCameraTrackingState::getInstance().s_distTransThres[level])) {
		return m_matrixTrackingLost;
	}

	Eigen::Matrix4f res; res.setIdentity();
	res.block(0, 0, 3, 3) = R;
	res.block(0, 3, 3, 1) = meanStDev*t+mean-R*mean;

	return res;
}
Esempio n. 24
0
RobotNodePtr RobotNodeFixed::_clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling)
{
	ReadLockPtr lock = getRobot()->getReadLock();
	
	RobotNodePtr result;

	Physics p = physics.scale(scaling);
	if (optionalDHParameter.isSet)
	{
		result.reset(new RobotNodeFixed(newRobot,name,optionalDHParameter.aMM()*scaling,optionalDHParameter.dMM()*scaling, optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(),visualizationModel,collisionModel,p,colChecker,nodeType));
	} else
	{
		Eigen::Matrix4f lt = getLocalTransformation();
		lt.block(0,3,3,1) *= scaling;
		result.reset(new RobotNodeFixed(newRobot,name,lt,visualizationModel,collisionModel,p,colChecker,nodeType));
	}

	return result;
}
Esempio n. 25
0
void SimDynamicsWindow::addObject()
{
    ManipulationObjectPtr vitalis;
    std::string vitalisPath = "objects/VitalisWithPrimitives.xml";

    if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(vitalisPath))
    {
        vitalis = ObjectIO::loadManipulationObject(vitalisPath);
    }

    if (vitalis)
    {
        vitalis->setMass(0.5f);
        float x, y, z;
        int counter = 0;

        do
        {
            x = float(rand() % 2000 - 1000);
            y = float(rand() % 2000 - 1000);
            z = float(rand() % 2000 + 100);
            Eigen::Matrix4f gp = vitalis->getGlobalPose();
            gp.block(0, 3, 3, 1) = Eigen::Vector3f(x, y, z);
            vitalis->setGlobalPose(gp);
            counter++;

            if (counter > 100)
            {
                cout << "Error, could not find valid pose" << endl;
                return;
            }
        }
        while (robot && CollisionChecker::getGlobalCollisionChecker()->checkCollision(robot, vitalis));

        SimDynamics::DynamicsObjectPtr dynamicsObjectVitalis = dynamicsWorld->CreateDynamicsObject(vitalis);
        dynamicsObjectVitalis->setPosition(Eigen::Vector3f(x, y, z));
        dynamicsObjects.push_back(dynamicsObjectVitalis);
        dynamicsWorld->addObject(dynamicsObjectVitalis);
        buildVisualization();
    }

}
Esempio n. 26
0
void compRollPitchCloud(pcl::PointCloud<PointXYZGD>::Ptr & cloud, double roll, double pitch)
{
	//map to points from perspective of body frame
    Eigen::Matrix4f trans;

#ifdef USE_QUATS
    trans.block(0,0,3,3) << curQuat.toRotationMatrix().cast<float>();
    trans(3,3) = 1;
#else
    trans << cos(roll), 0, sin(roll), 0,
             sin(roll)*sin(pitch), cos(pitch), -cos(roll)*sin(pitch), 0,
             -cos(pitch)*sin(roll), sin(pitch), cos(roll)*cos(pitch), 0,
             0, 0, 0, 1;
#endif

  //TODO: add translation
  //cout << trans << endl << endl;

  pcl::transformPointCloud(*cloud, *cloud_temp, trans);
  *cloud = *cloud_temp;
}
Esempio n. 27
0
    RobotNodePtr RobotNodePrismatic::_clone(const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling)
    {
        boost::shared_ptr<RobotNodePrismatic> result;
        ReadLockPtr lock = getRobot()->getReadLock();
        Physics p = physics.scale(scaling);

        if (optionalDHParameter.isSet)
        {
            result.reset(new RobotNodePrismatic(newRobot, name, jointLimitLo, jointLimitHi, optionalDHParameter.aMM()*scaling, optionalDHParameter.dMM()*scaling, optionalDHParameter.alphaRadian(), optionalDHParameter.thetaRadian(), visualizationModel, collisionModel, jointValueOffset, p, colChecker, nodeType));
        }
        else
        {
            Eigen::Matrix4f lt = getLocalTransformation();
            lt.block(0, 3, 3, 1) *= scaling;
            result.reset(new RobotNodePrismatic(newRobot, name, jointLimitLo, jointLimitHi, lt, jointTranslationDirection, visualizationModel, collisionModel, jointValueOffset, p, colChecker, nodeType));
        }

        if (result && visuScaling)
        {
            result->setVisuScaleFactor(visuScaleFactor);
        }

        return result;
    }
Esempio n. 28
0
osg::Node* OSGVisualizationFactory::getOSGVisualization( TriMeshModelPtr model, bool showNormals, VisualizationFactory::Color color )
{
	osg::Group *res = new osg::Group;
	res->ref();
	Eigen::Vector3f v1,v2,v3;
	for (size_t i=0;i<model->faces.size();i++)
	{
		v1 = model->vertices[model->faces[i].id1];
		v2 = model->vertices[model->faces[i].id2];
		v3 = model->vertices[model->faces[i].id3];
		//v2.setValue(model->vertices[model->faces[i].id2](0),model->vertices[model->faces[i].id2](1),model->vertices[model->faces[i].id2](2));
		//v3.setValue(model->vertices[model->faces[i].id3](0),model->vertices[model->faces[i].id3](1),model->vertices[model->faces[i].id3](2));
		std::vector<Eigen::Vector3f> v;
		v.push_back(v1);
		v.push_back(v2);
		v.push_back(v3);
		osg::Node* s = CreatePolygonVisualization(v,color);
		res->addChild(s);
		if (showNormals)
		{
			v1 = (v1 + v2 + v3) / 3.0f;
			osg::Group* ar = new osg::Group;
			Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
			mat.block(0,3,3,1) = v1;
			osg::MatrixTransform *mt = getMatrixTransform(mat);
			ar->addChild(mt);

			osg::Node *n = CreateArrow(model->faces[i].normal,30.0f,1.5f);
			mt->addChild(n);

			res->addChild(ar);
		}
	}
	res->unref_nodelete();
	return res;
}
/**
 * @brief Callback for feedback subscriber for getting the transformation of moved markers
 *
 * @param feedback subscribed from geometry_map/map/feedback
 */
void ShapeVisualization::setShapePosition(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)//,const cob_3d_mapping_msgs::Shape& shape)
{

  cob_3d_mapping_msgs::ShapeArray map_msg;
  map_msg.header.frame_id="/map";
  map_msg.header.stamp = ros::Time::now();

  int shape_id,index;
  index=-1;
  stringstream name(feedback->marker_name);

  Eigen::Quaternionf quat;

  Eigen::Matrix3f rotationMat;
  Eigen::MatrixXf rotationMatInit;

  Eigen::Vector3f normalVec;
  Eigen::Vector3f normalVecNew;
  Eigen::Vector3f newCentroid;
  Eigen::Matrix4f transSecondStep;


  if (feedback->marker_name != "Text"){
    name >> shape_id ;
    cob_3d_mapping::Polygon p;

    for(int i=0;i<sha.shapes.size();++i)
    {
    	if (sha.shapes[i].id == shape_id)
	{
		index = i;
	}

    }


    // temporary fix.
    //do nothing if index of shape is not found
    // this is not supposed to occur , but apparently it does
    if(index==-1){

    ROS_WARN("shape not in map array");
    return;
	}


    cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);

    if (feedback->event_type == 2 && feedback->menu_entry_id == 5){
      quatInit.x() = (float)feedback->pose.orientation.x ;           //normalized
      quatInit.y() = (float)feedback->pose.orientation.y ;
      quatInit.z() = (float)feedback->pose.orientation.z ;
      quatInit.w() = (float)feedback->pose.orientation.w ;

      oldCentroid (0) = (float)feedback->pose.position.x ;
      oldCentroid (1) = (float)feedback->pose.position.y ;
      oldCentroid (2) = (float)feedback->pose.position.z ;

      quatInit.normalize() ;

      rotationMatInit = quatInit.toRotationMatrix() ;

      transInit.block(0,0,3,3) << rotationMatInit ;
      transInit.col(3).head(3) << oldCentroid(0) , oldCentroid(1), oldCentroid(2) ;
      transInit.row(3) << 0,0,0,1 ;

      transInitInv = transInit.inverse() ;
      Eigen::Affine3f affineInitFinal (transInitInv) ;
      affineInit = affineInitFinal ;

      std::cout << "transInit : " << "\n"    << affineInitFinal.matrix() << "\n" ;
    }

    if (feedback->event_type == 5){
      /* the name of the marker is arrows_shape_.id, we need to erase the "arrows_" part */
      //string strName(feedback->marker_name);
      //strName.erase(strName.begin(),strName.begin()+7);
//      stringstream name(strName);
	stringstream name(feedback->marker_name);

      name >> shape_id ;
      cob_3d_mapping::Polygon p;
      cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);

      quat.x() = (float)feedback->pose.orientation.x ;           //normalized
      quat.y() = (float)feedback->pose.orientation.y ;
      quat.z() = (float)feedback->pose.orientation.z ;
      quat.w() = (float)feedback->pose.orientation.w ;

      quat.normalize() ;

      rotationMat = quat.toRotationMatrix() ;

      normalVec << sha.shapes.at(index).params[0],                   //normalized
          sha.shapes.at(index).params[1],
          sha.shapes.at(index).params[2];

      newCentroid << (float)feedback->pose.position.x ,
          (float)feedback->pose.position.y ,
          (float)feedback->pose.position.z ;


      transSecondStep.block(0,0,3,3) << rotationMat ;
      transSecondStep.col(3).head(3) << newCentroid(0) , newCentroid(1), newCentroid(2) ;
      transSecondStep.row(3) << 0,0,0,1 ;

      Eigen::Affine3f affineSecondStep(transSecondStep) ;

      std::cout << "transfrom : " << "\n"    << affineSecondStep.matrix() << "\n" ;

      Eigen::Affine3f affineFinal(affineSecondStep*affineInit) ;
      Eigen::Matrix4f matFinal = (transSecondStep*transInitInv) ;

      normalVecNew    = (matFinal.block(0,0,3,3))* normalVec;
      //      newCentroid  = transFinal *OldCentroid ;


      sha.shapes.at(index).centroid.x = newCentroid(0) ;
      sha.shapes.at(index).centroid.y = newCentroid(1) ;
      sha.shapes.at(index).centroid.z = newCentroid(2) ;


      sha.shapes.at(index).params[0] = normalVecNew(0) ;
      sha.shapes.at(index).params[1] = normalVecNew(1) ;
      sha.shapes.at(index).params[2] = normalVecNew(2) ;


      std::cout << "transfromFinal : " << "\n"    << affineFinal.matrix() << "\n" ;

      pcl::PointCloud<pcl::PointXYZ> pc;
      pcl::PointXYZ pt;
      sensor_msgs::PointCloud2 pc2;

      for(unsigned int j=0; j<p.contours.size(); j++)
      {
        for(unsigned int k=0; k<p.contours[j].size(); k++)
        {
          p.contours[j][k] = affineFinal * p.contours[j][k];
          pt.x = p.contours[j][k][0] ;
          pt.y = p.contours[j][k][1] ;
          pt.z = p.contours[j][k][2] ;
          pc.push_back(pt) ;
        }
      }

      pcl::toROSMsg (pc, pc2);
      sha.shapes.at(index).points.clear() ;
      sha.shapes.at(index).points.push_back (pc2);

      // uncomment when using test_shape_array

      //      for(unsigned int i=0;i<sha.shapes.size();i++){
      //        map_msg.header = sha.shapes.at(i).header ;
      //        map_msg.shapes.push_back(sha.shapes.at(i)) ;
      //      }
      //      shape_pub_.publish(map_msg);

      // end uncomment

      modified_shapes_.shapes.push_back(sha.shapes.at(index));
    }
Esempio n. 30
0
void SimoxMotionState::setGlobalPoseSimox( const Eigen::Matrix4f& worldPose )
{
	if (!sceneObject)
		return;

	// inv com as matrix4f
	Eigen::Matrix4f comLocal;
	comLocal.setIdentity();
	comLocal.block(0,3,3,1) = -com;

	// worldPose -> local visualization frame
	/*Eigen::Matrix4f localPose = sceneObject->getGlobalPoseVisualization().inverse() * worldPose;

	// com as matrix4f
	Eigen::Matrix4f comLocal;
	comLocal.setIdentity();
	comLocal.block(0,3,3,1) = -com;

	//Eigen::Matrix4f localPoseAdjusted =  localPose * comLocal;
	//Eigen::Matrix4f localPoseAdjusted =  comLocal * localPose;

	//Eigen::Matrix4f localPoseAdjusted =  localPose;
	//localPoseAdjusted.block(0,3,3,1) -= com;

    Eigen::Matrix4f comTrafo = Eigen::Matrix4f::Identity();
    comTrafo.block(0,3,3,1) = -com;
	Eigen::Matrix4f localPoseAdjusted =  localPose * comTrafo;
	//localPoseAdjusted.block(0,3,3,1) -= com;

	Eigen::Matrix4f resPose = sceneObject->getGlobalPoseVisualization() * localPoseAdjusted;
	*/
	Eigen::Matrix4f resPose = worldPose * comLocal;


	if (robotNodeActuator)
	{
        // get joint angle
        RobotNodePtr rn = robotNodeActuator->getRobotNode();
        DynamicsWorldPtr w = DynamicsWorld::GetWorld();
        DynamicsRobotPtr dr = w->getEngine()->getRobot(rn->getRobot());
        BulletRobotPtr bdr = boost::dynamic_pointer_cast<BulletRobot>(dr);
        if (bdr)
        {
            // check if robotnode is connected with a joint
            std::vector<BulletRobot::LinkInfo> links = bdr->getLinks(rn);
            // update all involved joint values
            for (size_t i=0;i<links.size();i++)
            {
                if (links[i].nodeJoint)
                {
                    float ja = bdr->getJointAngle(links[i].nodeJoint);
                    // we can update the joint value via an RobotNodeActuator
                    RobotNodeActuatorPtr rna (new RobotNodeActuator(links[i].nodeJoint));
                    rna->updateJointAngle(ja);
                }
            }

		    // we assume that all models are handled by Bullet, so we do not need to update children
		    robotNodeActuator->updateVisualizationPose(resPose, false); 
#if 0
            if (rn->getName() == "Shoulder 1 L")
            {
                cout << "Shoulder 1 L:" << ja << ", speed:" << bdr->getJointSpeed(rn) << endl;
            }

#endif
        } /*else
        {
            VR_WARNING << "Could not determine dynamic robot?!" << endl;
        }*/
	} else
	{
		sceneObject->setGlobalPose(resPose);
	}
}