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;
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;
}
/** @name Test_Cases
For each of these cases, we check if the accurate forces are computed
*/
DiagonalStiffness_test():Inherited::ForceField_test()
{
this->errorFactorPotentialEnergy = 3; // increading tolerance for potential energy test due to non-linearities
//Position
x.resize(3);
DataTypes::set( x[0], 0,0,0);
DataTypes::set( x[1], -1,0,0);
DataTypes::set( x[2], 7,0,0);
//Velocity
v.resize(3);
DataTypes::set( v[0], 0,0,0);
DataTypes::set( v[1], 0,0,0);
DataTypes::set( v[2], 0,0,0);
//Force
f.resize(3);
DataTypes::set( f[0], 0,0,0 );
DataTypes::set( f[1], 100,0,0 );
DataTypes::set( f[2], -49,0,0 );
// Set force parameters
VecDeriv& stiffnesses = *Inherited::force->diagonal.beginWriteOnly();
stiffnesses.resize(3);
stiffnesses[0] = 10;
stiffnesses[1] = 100;
stiffnesses[2] = 7;
Inherited::force->diagonal.endEdit();
}
TrianglePressureForceField_test(): Inherited::ForceField_test(std::string(SOFABOUNDARYCONDITION_TEST_SCENES_DIR) + "/" + "TrianglePressureForceField.scn")
{
// potential energy is not implemented and won't be tested
this->flags &= ~Inherited::TEST_POTENTIAL_ENERGY;
// Set vectors, using DataTypes::set to cope with tests in dimension 2
//Position
x.resize(3);
DataTypes::set( x[0], 0,0,0);
DataTypes::set( x[1], 1,0,0);
DataTypes::set( x[2], 1,1,0);
//Velocity
v.resize(3);
DataTypes::set( v[0], 0,0,0);
DataTypes::set( v[1], 0,0,0);
DataTypes::set( v[2], 0,0,0);
//Force
f.resize(3);
Vec3 f0(0,0,0.1);
DataTypes::set( f[0], f0[0], f0[1], f0[2]);
DataTypes::set( f[1], f0[0], f0[1], f0[2]);
DataTypes::set( f[2], f0[0], f0[1], f0[2]);
// Set the properties of the force field
Inherited::force->normal.setValue(Deriv(0,0,1));
Inherited::force->dmin.setValue(-0.01);
Inherited::force->dmax.setValue(0.01);
Inherited::force->pressure=Coord(0,0,0.6);
}
void ClosestPointRegistrationForceFieldCam<DataTypes, DepthTypes>::extractTargetPCD()
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr target(new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr target;
target = PCDFromRGBD(depth,foreground);
pcl::PointCloud<pcl::PointXYZRGB> target0;
pcl::io::loadPCDFile ("/home/apetit/soft/catkin_ws/test_pcd12.pcd", target0);
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr target;
VecCoord targetpos;
targetpos.resize(target->size());
Vector3 pos;
Vector3 col;
for (unsigned int i=0; i<target->size(); i++)
{
pos[0] = target->points[i].x;
pos[1] = target->points[i].y;
pos[2] = target->points[i].z;
targetpos[i]=pos;
//std::cout << " x " << pos[0] << " y " << pos[1] << " z " << pos[2] << std::endl;
}
/*for (unsigned int i=target->size(); i<targetpos.size(); i++)
{
pos[0] = target0[i].x;
pos[1] = target0[i].y;
pos[2] = target0[i].z;
targetpos[i]=pos;
//std::cout << " x " << pos[0] << " y " << pos[1] << " z " << pos[2] << std::endl;
}*/
//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;
}
