/// After simulation compare the positions of points to the theoretical positions. bool compareSimulatedToTheoreticalPositions(double convergenceAccuracy, double diffMaxBetweenSimulatedAndTheoreticalPosition) { // Init simulation sofa::simulation::getSimulation()->init(root.get()); // Compute the theoretical final positions VecCoord finalPos; typename PatchTestMovementConstraint::SPtr bilinearConstraint = root->get<PatchTestMovementConstraint>(root->SearchDown); typename MechanicalObject::SPtr dofs = root->get<MechanicalObject>(root->SearchDown); typename MechanicalObject::ReadVecCoord x0 = dofs->readPositions(); bilinearConstraint->getFinalPositions( finalPos,*dofs->write(core::VecCoordId::position()) ); // Initialize size_t numNodes = finalPos.size(); VecCoord xprev(numNodes); VecDeriv dx(numNodes); bool hasConverged = true; for (size_t i=0; i<numNodes; i++) { xprev[i] = CPos(0,0,0); } // Animate do { hasConverged = true; sofa::simulation::getSimulation()->animate(root.get(),0.5); typename MechanicalObject::ReadVecCoord x = dofs->readPositions(); // Compute dx for (size_t i=0; i<x.size(); i++) { dx[i] = x[i]-xprev[i]; // Test convergence if(dx[i].norm()>convergenceAccuracy) hasConverged = false; } // xprev = x for (size_t i=0; i<numNodes; i++) { xprev[i]=x[i]; } } while(!hasConverged); // not converged // Compare the theoretical positions and the simulated positions bool succeed=true; for(size_t i=0; i<finalPos.size(); i++ ) { if((finalPos[i]-x0[i]).norm()>diffMaxBetweenSimulatedAndTheoreticalPosition) { succeed = false; ADD_FAILURE() << "final Position of point " << i << " is wrong: " << x0[i] << std::endl <<"the expected Position is " << finalPos[i] << std::endl << "difference = " <<(finalPos[i]-x0[i]).norm() << std::endl; } } return succeed; }
void ClosestPointRegistrationForceFieldCam<DataTypes, DepthTypes>::initTarget() { //pcl::PointCloud<pcl::PointXYZRGB>::Ptr target (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB> target; pcl::io::loadPCDFile ("/home/apetit/soft/catkin_ws/test_pcd12.pcd", target); VecCoord targetpos; targetpos.resize(target.size()); Vector3 pos; Vector3 col; for (unsigned int i=0; i<targetpos.size(); i++) { pos[0] = target[i].x; pos[1] = target[i].y; pos[2] = target[i].z; targetpos[i]=pos; } //targetPositions.setValue(targetpos); // build k-d tree //const VecCoord& p = targetPositions.getValue(); const VecCoord& p = targetpos; targetPositions.setValue(p); targetKdTree.build(p); // detect border if(targetBorder.size()!=p.size()) { targetBorder.resize(p.size()); detectBorder(targetBorder,targetTriangles.getValue()); } }
/// After simulation compare the positions of points to the theoretical positions. bool compareSimulatedToTheoreticalPositions(double convergenceAccuracy, double diffMaxBetweenSimulatedAndTheoreticalPosition) { // Init simulation sofa::simulation::getSimulation()->init(root.get()); // Compute the theoretical final positions VecCoord finalPos; patchStruct.affineConstraint->getFinalPositions( finalPos,*patchStruct.dofs->write(core::VecCoordId::position()) ); // Initialize size_t numNodes = finalPos.size(); VecCoord xprev(numNodes); VecDeriv dx(numNodes); bool hasConverged = true; for (size_t i=0; i<numNodes; i++) { xprev[i] = CPos(0,0,0); } // Animate do { hasConverged = true; sofa::simulation::getSimulation()->animate(root.get(),0.5); typename MechanicalObject::ReadVecCoord x = patchStruct.dofs->readPositions(); // Compute dx for (size_t i=0; i<x.size(); i++) { dx[i] = x[i]-xprev[i]; // Test convergence if(dx[i].norm()>convergenceAccuracy) hasConverged = false; } // xprev = x for (size_t i=0; i<numNodes; i++) { xprev[i]=x[i]; } } while(!hasConverged); // not converged // Get simulated positions typename MechanicalObject::WriteVecCoord x = patchStruct.dofs->writePositions(); // Compare the theoretical positions and the simulated positions bool succeed=true; for(size_t i=0; i<finalPos.size(); i++ ) { if((finalPos[i]-x[i]).norm()>diffMaxBetweenSimulatedAndTheoreticalPosition) { succeed = false; ADD_FAILURE() << "final Position of point " << i << " is wrong: " << x[i] << std::endl <<"the expected Position is " << finalPos[i] << std::endl << "difference = " <<(finalPos[i]-x[i]).norm() << std::endl <<"rotation = " << testedRotation << std::endl << " translation = " << testedTranslation << std::endl << "Failed seed number =" << BaseSofa_test::seed; } } return succeed; }