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