CV_IMPL void
cvCalcOpticalFlowPyrLK( const void* arrA, const void* arrB,
void* /*pyrarrA*/, void* /*pyrarrB*/,
const CvPoint2D32f * featuresA,
CvPoint2D32f * featuresB,
int count, CvSize winSize, int level,
char *status, float *error,
CvTermCriteria criteria, int flags )
{
if( count <= 0 )
return;
CV_Assert( featuresA && featuresB );
cv::Mat A = cv::cvarrToMat(arrA), B = cv::cvarrToMat(arrB);
cv::Mat ptA(count, 1, CV_32FC2, (void*)featuresA);
cv::Mat ptB(count, 1, CV_32FC2, (void*)featuresB);
cv::Mat st, err;
if( status )
st = cv::Mat(count, 1, CV_8U, (void*)status);
if( error )
err = cv::Mat(count, 1, CV_32F, (void*)error);
cv::calcOpticalFlowPyrLK( A, B, ptA, ptB, st,
error ? cv::_OutputArray(err) : (cv::_OutputArray)cv::noArray(),
winSize, level, criteria, flags);
}
void VTKRecorder::action(){
vector<bool> recActive(REC_SENTINEL,false);
FOREACH(string& rec, recorders){
if(rec=="all"){
recActive[REC_SPHERES]=true;
recActive[REC_VELOCITY]=true;
recActive[REC_FACETS]=true;
recActive[REC_COLORS]=true;
recActive[REC_MASS]=true;
recActive[REC_INTR]=true;
recActive[REC_ID]=true;
recActive[REC_MASK]=true;
recActive[REC_CLUMPID]=true;
recActive[REC_MATERIALID]=true;
recActive[REC_STRESS]=true;
}
else if(rec=="spheres") recActive[REC_SPHERES]=true;
else if(rec=="velocity") recActive[REC_VELOCITY]=true;
else if(rec=="facets") recActive[REC_FACETS]=true;
else if(rec=="mass") recActive[REC_MASS]=true;
else if((rec=="colors") || (rec=="color"))recActive[REC_COLORS]=true;
else if(rec=="cpm") recActive[REC_CPM]=true;
else if(rec=="rpm") recActive[REC_RPM]=true;
else if(rec=="wpm") recActive[REC_WPM]=true;
else if(rec=="intr") recActive[REC_INTR]=true;
else if((rec=="ids") || (rec=="id")) recActive[REC_ID]=true;
else if(rec=="mask") recActive[REC_MASK]=true;
else if((rec=="clumpids") || (rec=="clumpId")) recActive[REC_CLUMPID]=true;
else if(rec=="materialId") recActive[REC_MATERIALID]=true;
else if(rec=="stress") recActive[REC_STRESS]=true;
else LOG_ERROR("Unknown recorder named `"<<rec<<"' (supported are: all, spheres, velocity, facets, color, stress, cpm, rpm, wpm, intr, id, clumpId, materialId). Ignored.");
}
// cpm needs interactions
if(recActive[REC_CPM]) recActive[REC_INTR]=true;
// wpm needs interactions
if(recActive[REC_WPM]) recActive[REC_INTR]=true;
// spheres
vtkSmartPointer<vtkPoints> spheresPos = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkCellArray> spheresCells = vtkSmartPointer<vtkCellArray>::New();
vtkSmartPointer<vtkFloatArray> radii = vtkSmartPointer<vtkFloatArray>::New();
radii->SetNumberOfComponents(1);
radii->SetName("radii");
vtkSmartPointer<vtkFloatArray> spheresMass = vtkSmartPointer<vtkFloatArray>::New();
spheresMass->SetNumberOfComponents(1);
spheresMass->SetName("mass");
vtkSmartPointer<vtkFloatArray> spheresId = vtkSmartPointer<vtkFloatArray>::New();
spheresId->SetNumberOfComponents(1);
spheresId->SetName("id");
vtkSmartPointer<vtkFloatArray> spheresMask = vtkSmartPointer<vtkFloatArray>::New();
spheresMask->SetNumberOfComponents(1);
spheresMask->SetName("mask");
vtkSmartPointer<vtkFloatArray> clumpId = vtkSmartPointer<vtkFloatArray>::New();
clumpId->SetNumberOfComponents(1);
clumpId->SetName("clumpId");
vtkSmartPointer<vtkFloatArray> spheresColors = vtkSmartPointer<vtkFloatArray>::New();
spheresColors->SetNumberOfComponents(3);
spheresColors->SetName("color");
vtkSmartPointer<vtkFloatArray> spheresLinVelVec = vtkSmartPointer<vtkFloatArray>::New();
spheresLinVelVec->SetNumberOfComponents(3);
spheresLinVelVec->SetName("linVelVec"); //Linear velocity in Vector3 form
vtkSmartPointer<vtkFloatArray> spheresLinVelLen = vtkSmartPointer<vtkFloatArray>::New();
spheresLinVelLen->SetNumberOfComponents(1);
spheresLinVelLen->SetName("linVelLen"); //Length (magnitude) of linear velocity
vtkSmartPointer<vtkFloatArray> spheresAngVelVec = vtkSmartPointer<vtkFloatArray>::New();
spheresAngVelVec->SetNumberOfComponents(3);
spheresAngVelVec->SetName("angVelVec"); //Angular velocity in Vector3 form
vtkSmartPointer<vtkFloatArray> spheresAngVelLen = vtkSmartPointer<vtkFloatArray>::New();
spheresAngVelLen->SetNumberOfComponents(1);
spheresAngVelLen->SetName("angVelLen"); //Length (magnitude) of angular velocity
vtkSmartPointer<vtkFloatArray> spheresNormalStressVec = vtkSmartPointer<vtkFloatArray>::New();
spheresNormalStressVec->SetNumberOfComponents(3);
spheresNormalStressVec->SetName("normalStress");
vtkSmartPointer<vtkFloatArray> spheresShearStressVec = vtkSmartPointer<vtkFloatArray>::New();
spheresShearStressVec->SetNumberOfComponents(3);
spheresShearStressVec->SetName("shearStress");
vtkSmartPointer<vtkFloatArray> spheresNormalStressNorm = vtkSmartPointer<vtkFloatArray>::New();
spheresNormalStressNorm->SetNumberOfComponents(1);
spheresNormalStressNorm->SetName("normalStressNorm");
vtkSmartPointer<vtkFloatArray> spheresMaterialId = vtkSmartPointer<vtkFloatArray>::New();
spheresMaterialId->SetNumberOfComponents(1);
spheresMaterialId->SetName("materialId");
// facets
vtkSmartPointer<vtkPoints> facetsPos = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkCellArray> facetsCells = vtkSmartPointer<vtkCellArray>::New();
vtkSmartPointer<vtkFloatArray> facetsColors = vtkSmartPointer<vtkFloatArray>::New();
facetsColors->SetNumberOfComponents(3);
facetsColors->SetName("color");
vtkSmartPointer<vtkFloatArray> facetsForceVec = vtkSmartPointer<vtkFloatArray>::New();
facetsForceVec->SetNumberOfComponents(3);
facetsForceVec->SetName("stressVec");
vtkSmartPointer<vtkFloatArray> facetsForceLen = vtkSmartPointer<vtkFloatArray>::New();
facetsForceLen->SetNumberOfComponents(1);
facetsForceLen->SetName("stressLen");
vtkSmartPointer<vtkFloatArray> facetsMaterialId = vtkSmartPointer<vtkFloatArray>::New();
facetsMaterialId->SetNumberOfComponents(1);
facetsMaterialId->SetName("materialId");
vtkSmartPointer<vtkFloatArray> facetsMask = vtkSmartPointer<vtkFloatArray>::New();
facetsMask->SetNumberOfComponents(1);
facetsMask->SetName("mask");
// interactions
vtkSmartPointer<vtkPoints> intrBodyPos = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkCellArray> intrCells = vtkSmartPointer<vtkCellArray>::New();
vtkSmartPointer<vtkFloatArray> intrForceN = vtkSmartPointer<vtkFloatArray>::New();
intrForceN->SetNumberOfComponents(1);
intrForceN->SetName("forceN");
vtkSmartPointer<vtkFloatArray> intrAbsForceT = vtkSmartPointer<vtkFloatArray>::New();
intrAbsForceT->SetNumberOfComponents(3);
intrAbsForceT->SetName("absForceT");
// extras for CPM
if(recActive[REC_CPM]){ CpmStateUpdater csu; csu.update(scene); }
vtkSmartPointer<vtkFloatArray> cpmDamage = vtkSmartPointer<vtkFloatArray>::New();
cpmDamage->SetNumberOfComponents(1);
cpmDamage->SetName("cpmDamage");
vtkSmartPointer<vtkFloatArray> cpmStress = vtkSmartPointer<vtkFloatArray>::New();
cpmStress->SetNumberOfComponents(9);
cpmStress->SetName("cpmStress");
// extras for RPM
vtkSmartPointer<vtkFloatArray> rpmSpecNum = vtkSmartPointer<vtkFloatArray>::New();
rpmSpecNum->SetNumberOfComponents(1);
rpmSpecNum->SetName("rpmSpecNum");
vtkSmartPointer<vtkFloatArray> rpmSpecMass = vtkSmartPointer<vtkFloatArray>::New();
rpmSpecMass->SetNumberOfComponents(1);
rpmSpecMass->SetName("rpmSpecMass");
vtkSmartPointer<vtkFloatArray> rpmSpecDiam = vtkSmartPointer<vtkFloatArray>::New();
rpmSpecDiam->SetNumberOfComponents(1);
rpmSpecDiam->SetName("rpmSpecDiam");
// extras for WireMatPM
vtkSmartPointer<vtkFloatArray> wpmNormalForce = vtkSmartPointer<vtkFloatArray>::New();
wpmNormalForce->SetNumberOfComponents(1);
wpmNormalForce->SetName("wpmNormalForce");
vtkSmartPointer<vtkFloatArray> wpmLimitFactor = vtkSmartPointer<vtkFloatArray>::New();
wpmLimitFactor->SetNumberOfComponents(1);
wpmLimitFactor->SetName("wpmLimitFactor");
if(recActive[REC_INTR]){
// holds information about cell distance between spatial and displayed position of each particle
vector<Vector3i> wrapCellDist; if (scene->isPeriodic){ wrapCellDist.resize(scene->bodies->size()); }
// save body positions, referenced by ids by vtkLine
FOREACH(const shared_ptr<Body>& b, *scene->bodies){
if (!b) {
/* must keep ids contiguous, so that position in the array corresponds to Body::id */
intrBodyPos->InsertNextPoint(NaN,NaN,NaN);
continue;
}
if(!scene->isPeriodic){ intrBodyPos->InsertNextPoint(b->state->pos[0],b->state->pos[1],b->state->pos[2]); }
else {
Vector3r pos=scene->cell->wrapShearedPt(b->state->pos,wrapCellDist[b->id]);
intrBodyPos->InsertNextPoint(pos[0],pos[1],pos[2]);
}
assert(intrBodyPos->GetNumberOfPoints()==b->id+1);
}
FOREACH(const shared_ptr<Interaction>& I, *scene->interactions){
if(!I->isReal()) continue;
if(skipFacetIntr){
if(!(Body::byId(I->getId1()))) continue;
if(!(Body::byId(I->getId2()))) continue;
if(!(dynamic_cast<Sphere*>(Body::byId(I->getId1())->shape.get()))) continue;
if(!(dynamic_cast<Sphere*>(Body::byId(I->getId2())->shape.get()))) continue;
}
/* For the periodic boundary conditions,
find out whether the interaction crosses the boundary of the periodic cell;
if it does, display the interaction on both sides of the cell, with one of the
points sticking out in each case.
Since vtkLines must connect points with an ID assigned, we will create a new additional
point for each point outside the cell. It might create some data redundancy, but
let us suppose that the number of interactions crossing the cell boundary is low compared
to total numer of interactions
*/
// how many times to add values defined on interactions, depending on how many times the interaction is saved
int numAddValues=1;
// aperiodic boundary, or interaction is inside the cell
if(!scene->isPeriodic || (scene->isPeriodic && (I->cellDist==wrapCellDist[I->getId2()]-wrapCellDist[I->getId1()]))){
vtkSmartPointer<vtkLine> line = vtkSmartPointer<vtkLine>::New();
line->GetPointIds()->SetId(0,I->getId1());
line->GetPointIds()->SetId(1,I->getId2());
intrCells->InsertNextCell(line);
} else {
assert(scene->isPeriodic);
// spatial positions of particles
const Vector3r& p01(Body::byId(I->getId1())->state->pos); const Vector3r& p02(Body::byId(I->getId2())->state->pos);
// create two line objects; each of them has one endpoint inside the cell and the other one sticks outside
// A,B are the "fake" bodies outside the cell for id1 and id2 respectively, p1,p2 are the displayed points
// distance in cell units for shifting A away from p1; negated value is shift of B away from p2
Vector3r ptA(p01+scene->cell->hSize*(wrapCellDist[I->getId2()]-I->cellDist).cast<Real>());
Vector3r ptB(p02+scene->cell->hSize*(wrapCellDist[I->getId1()]-I->cellDist).cast<Real>());
vtkIdType idPtA=intrBodyPos->InsertNextPoint(ptA[0],ptA[1],ptA[2]), idPtB=intrBodyPos->InsertNextPoint(ptB[0],ptB[1],ptB[2]);
vtkSmartPointer<vtkLine> line1B(vtkSmartPointer<vtkLine>::New()); line1B->GetPointIds()->SetId(0,I->getId1()); line1B->GetPointIds()->SetId(1,idPtB);
vtkSmartPointer<vtkLine> lineA2(vtkSmartPointer<vtkLine>::New()); lineA2->GetPointIds()->SetId(0,idPtA); lineA2->GetPointIds()->SetId(1,I->getId2());
numAddValues=2;
}
const NormShearPhys* phys = YADE_CAST<NormShearPhys*>(I->phys.get());
const GenericSpheresContact* geom = YADE_CAST<GenericSpheresContact*>(I->geom.get());
// gives _signed_ scalar of normal force, following the convention used in the respective constitutive law
float fn=phys->normalForce.dot(geom->normal);
float fs[3]={abs(phys->shearForce[0]),abs(phys->shearForce[1]),abs(phys->shearForce[2])};
// add the value once for each interaction object that we created (might be 2 for the periodic boundary)
for(int i=0; i<numAddValues; i++){
intrAbsForceT->InsertNextTupleValue(fs);
if(recActive[REC_WPM]) {
const WirePhys* wirephys = dynamic_cast<WirePhys*>(I->phys.get());
if (wirephys!=NULL && wirephys->isLinked) {
wpmLimitFactor->InsertNextValue(wirephys->limitFactor);
wpmNormalForce->InsertNextValue(fn);
intrForceN->InsertNextValue(NaN);
}
else {
intrForceN->InsertNextValue(fn);
wpmNormalForce->InsertNextValue(NaN);
wpmLimitFactor->InsertNextValue(NaN);
}
}
else {
intrForceN->InsertNextValue(fn);
}
}
}
}
//Additional Vector for storing forces
vector<Shop::bodyState> bodyStates;
if(recActive[REC_STRESS]) Shop::getStressForEachBody(bodyStates);
FOREACH(const shared_ptr<Body>& b, *scene->bodies){
if (!b) continue;
if(mask!=0 && (b->groupMask & mask)==0) continue;
if (recActive[REC_SPHERES]){
const Sphere* sphere = dynamic_cast<Sphere*>(b->shape.get());
if (sphere){
if(skipNondynamic && b->state->blockedDOFs==State::DOF_ALL) continue;
vtkIdType pid[1];
Vector3r pos(scene->isPeriodic ? scene->cell->wrapShearedPt(b->state->pos) : b->state->pos);
pid[0] = spheresPos->InsertNextPoint(pos[0], pos[1], pos[2]);
spheresCells->InsertNextCell(1,pid);
radii->InsertNextValue(sphere->radius);
if (recActive[REC_ID]) spheresId->InsertNextValue(b->getId());
if (recActive[REC_MASK]) spheresMask->InsertNextValue(b->groupMask);
if (recActive[REC_MASS]) spheresMass->InsertNextValue(b->state->mass);
if (recActive[REC_CLUMPID]) clumpId->InsertNextValue(b->clumpId);
if (recActive[REC_COLORS]){
const Vector3r& color = sphere->color;
float c[3] = {color[0],color[1],color[2]};
spheresColors->InsertNextTupleValue(c);
}
if(recActive[REC_VELOCITY]){
const Vector3r& vel = b->state->vel;
float v[3] = { vel[0],vel[1],vel[2] };
spheresLinVelVec->InsertNextTupleValue(v);
spheresLinVelLen->InsertNextValue(vel.norm());
const Vector3r& angVel = b->state->angVel;
float av[3] = { angVel[0],angVel[1],angVel[2] };
spheresAngVelVec->InsertNextTupleValue(av);
spheresAngVelLen->InsertNextValue(angVel.norm());
}
if(recActive[REC_STRESS]){
const Vector3r& stress = bodyStates[b->getId()].normStress;
const Vector3r& shear = bodyStates[b->getId()].shearStress;
float n[3] = { stress[0],stress[1],stress[2] };
float s[3] = { shear [0],shear [1],shear [2] };
spheresNormalStressVec->InsertNextTupleValue(n);
spheresShearStressVec->InsertNextTupleValue(s);
spheresNormalStressNorm->InsertNextValue(stress.norm());
}
if (recActive[REC_CPM]){
cpmDamage->InsertNextValue(YADE_PTR_CAST<CpmState>(b->state)->normDmg);
const Matrix3r& ss=YADE_PTR_CAST<CpmState>(b->state)->stress;
//float s[3]={ss[0],ss[1],ss[2]};
float s[9]={ss[0,0],ss[0,1],ss[0,2],ss[1,0],ss[1,1],ss[1,2],ss[2,0],ss[2,1],ss[2,2]};
cpmStress->InsertNextTupleValue(s);
}
if (recActive[REC_RPM]){
rpmSpecNum->InsertNextValue(YADE_PTR_CAST<RpmState>(b->state)->specimenNumber);
rpmSpecMass->InsertNextValue(YADE_PTR_CAST<RpmState>(b->state)->specimenMass);
rpmSpecDiam->InsertNextValue(YADE_PTR_CAST<RpmState>(b->state)->specimenMaxDiam);
}
if (recActive[REC_MATERIALID]) spheresMaterialId->InsertNextValue(b->material->id);
continue;
}
}
if (recActive[REC_FACETS]){
const Facet* facet = dynamic_cast<Facet*>(b->shape.get());
if (facet){
Vector3r pos(scene->isPeriodic ? scene->cell->wrapShearedPt(b->state->pos) : b->state->pos);
const vector<Vector3r>& localPos = facet->vertices;
Matrix3r facetAxisT=b->state->ori.toRotationMatrix();
vtkSmartPointer<vtkTriangle> tri = vtkSmartPointer<vtkTriangle>::New();
vtkIdType nbPoints=facetsPos->GetNumberOfPoints();
for (int i=0;i<3;++i){
Vector3r globalPos = pos + facetAxisT * localPos[i];
facetsPos->InsertNextPoint(globalPos[0], globalPos[1], globalPos[2]);
tri->GetPointIds()->SetId(i,nbPoints+i);
}
facetsCells->InsertNextCell(tri);
if (recActive[REC_COLORS]){
const Vector3r& color = facet->color;
float c[3] = {color[0],color[1],color[2]};
facetsColors->InsertNextTupleValue(c);
}
if(recActive[REC_STRESS]){
const Vector3r& stress = bodyStates[b->getId()].normStress+bodyStates[b->getId()].shearStress;
float s[3] = { stress[0],stress[1],stress[2] };
facetsForceVec->InsertNextTupleValue(s);
facetsForceLen->InsertNextValue(stress.norm());
}
if (recActive[REC_MATERIALID]) facetsMaterialId->InsertNextValue(b->material->id);
if (recActive[REC_MASK]) facetsMask->InsertNextValue(b->groupMask);
continue;
}
}
}
vtkSmartPointer<vtkDataCompressor> compressor;
if(compress) compressor=vtkSmartPointer<vtkZLibDataCompressor>::New();
vtkSmartPointer<vtkUnstructuredGrid> spheresUg = vtkSmartPointer<vtkUnstructuredGrid>::New();
if (recActive[REC_SPHERES]){
spheresUg->SetPoints(spheresPos);
spheresUg->SetCells(VTK_VERTEX, spheresCells);
spheresUg->GetPointData()->AddArray(radii);
if (recActive[REC_ID]) spheresUg->GetPointData()->AddArray(spheresId);
if (recActive[REC_MASK]) spheresUg->GetPointData()->AddArray(spheresMask);
if (recActive[REC_MASS]) spheresUg->GetPointData()->AddArray(spheresMass);
if (recActive[REC_CLUMPID]) spheresUg->GetPointData()->AddArray(clumpId);
if (recActive[REC_COLORS]) spheresUg->GetPointData()->AddArray(spheresColors);
if (recActive[REC_VELOCITY]){
spheresUg->GetPointData()->AddArray(spheresLinVelVec);
spheresUg->GetPointData()->AddArray(spheresAngVelVec);
spheresUg->GetPointData()->AddArray(spheresLinVelLen);
spheresUg->GetPointData()->AddArray(spheresAngVelLen);
}
if (recActive[REC_STRESS]){
spheresUg->GetPointData()->AddArray(spheresNormalStressVec);
spheresUg->GetPointData()->AddArray(spheresShearStressVec);
spheresUg->GetPointData()->AddArray(spheresNormalStressNorm);
}
if (recActive[REC_CPM]){
spheresUg->GetPointData()->AddArray(cpmDamage);
spheresUg->GetPointData()->AddArray(cpmStress);
}
if (recActive[REC_RPM]){
spheresUg->GetPointData()->AddArray(rpmSpecNum);
spheresUg->GetPointData()->AddArray(rpmSpecMass);
spheresUg->GetPointData()->AddArray(rpmSpecDiam);
}
if (recActive[REC_MATERIALID]) spheresUg->GetPointData()->AddArray(spheresMaterialId);
#ifdef YADE_VTK_MULTIBLOCK
if(!multiblock)
#endif
{
vtkSmartPointer<vtkXMLUnstructuredGridWriter> writer = vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
if(compress) writer->SetCompressor(compressor);
if(ascii) writer->SetDataModeToAscii();
string fn=fileName+"spheres."+lexical_cast<string>(scene->iter)+".vtu";
writer->SetFileName(fn.c_str());
writer->SetInput(spheresUg);
writer->Write();
}
}
vtkSmartPointer<vtkUnstructuredGrid> facetsUg = vtkSmartPointer<vtkUnstructuredGrid>::New();
if (recActive[REC_FACETS]){
facetsUg->SetPoints(facetsPos);
facetsUg->SetCells(VTK_TRIANGLE, facetsCells);
if (recActive[REC_COLORS]) facetsUg->GetCellData()->AddArray(facetsColors);
if (recActive[REC_STRESS]){
facetsUg->GetCellData()->AddArray(facetsForceVec);
facetsUg->GetCellData()->AddArray(facetsForceLen);
}
if (recActive[REC_MATERIALID]) facetsUg->GetCellData()->AddArray(facetsMaterialId);
if (recActive[REC_MASK]) facetsUg->GetCellData()->AddArray(facetsMask);
#ifdef YADE_VTK_MULTIBLOCK
if(!multiblock)
#endif
{
vtkSmartPointer<vtkXMLUnstructuredGridWriter> writer = vtkSmartPointer<vtkXMLUnstructuredGridWriter>::New();
if(compress) writer->SetCompressor(compressor);
if(ascii) writer->SetDataModeToAscii();
string fn=fileName+"facets."+lexical_cast<string>(scene->iter)+".vtu";
writer->SetFileName(fn.c_str());
writer->SetInput(facetsUg);
writer->Write();
}
}
vtkSmartPointer<vtkPolyData> intrPd = vtkSmartPointer<vtkPolyData>::New();
if (recActive[REC_INTR]){
intrPd->SetPoints(intrBodyPos);
intrPd->SetLines(intrCells);
intrPd->GetCellData()->AddArray(intrForceN);
intrPd->GetCellData()->AddArray(intrAbsForceT);
if (recActive[REC_WPM]){
intrPd->GetCellData()->AddArray(wpmNormalForce);
intrPd->GetCellData()->AddArray(wpmLimitFactor);
}
#ifdef YADE_VTK_MULTIBLOCK
if(!multiblock)
#endif
{
vtkSmartPointer<vtkXMLPolyDataWriter> writer = vtkSmartPointer<vtkXMLPolyDataWriter>::New();
if(compress) writer->SetCompressor(compressor);
if(ascii) writer->SetDataModeToAscii();
string fn=fileName+"intrs."+lexical_cast<string>(scene->iter)+".vtp";
writer->SetFileName(fn.c_str());
writer->SetInput(intrPd);
writer->Write();
}
}
#ifdef YADE_VTK_MULTIBLOCK
if(multiblock){
vtkSmartPointer<vtkMultiBlockDataSet> multiblockDataset = vtkSmartPointer<vtkMultiBlockDataSet>::New();
int i=0;
if(recActive[REC_SPHERES]) multiblockDataset->SetBlock(i++,spheresUg);
if(recActive[REC_FACETS]) multiblockDataset->SetBlock(i++,facetsUg);
if(recActive[REC_INTR]) multiblockDataset->SetBlock(i++,intrPd);
vtkSmartPointer<vtkXMLMultiBlockDataWriter> writer = vtkSmartPointer<vtkXMLMultiBlockDataWriter>::New();
if(ascii) writer->SetDataModeToAscii();
string fn=fileName+lexical_cast<string>(scene->iter)+".vtm";
writer->SetFileName(fn.c_str());
writer->SetInput(multiblockDataset);
writer->Write();
}
#endif
}
int main(int argc, char *argv[]) {
cout << "Point A is created by ";
grid3d ptA(3, 4, 5);
cout << "Point A is ";
ptA.print();
cout << endl << endl;
cout << "Point B is created by ";
grid3d ptB = 2;
cout << "Point B is ";
ptB.print();
cout << endl << endl;
cout << "Point C is created by ";
grid3d ptC;
ptC = -6;
cout << "Point C is ";
ptC.print();
cout << endl << endl;
cout << "Point D is created by ";
grid3d ptD(ptA);
cout << "Point D is ";
ptD.print();
cout << endl << endl;
cout << "Point E is created by ";
grid3d ptE = grid3d(-2,2,7);
cout << "Point E is ";
ptE.print();
cout << endl << endl;
cout << "Creating an array of points... "<<endl;
grid3d ptArr[2];
cout << "The points in the array are ";
ptArr[0].print();
ptArr[1].print();
cout << endl << endl;
cout << "Please enter three integers to set a point in the array ";
cin >> ptArr[0];
cout << "The point is set to ";
ptArr[0].print();
cout << endl << endl;
cout << "Start to compute centroid" << endl;
grid3d centroid = findCentroid(ptA, ptB, ptE);
cout << "The centroid of triange ";
ptA.print(); ptB.print(); ptE.print();
cout << " is ";
centroid.print();
cout << endl << endl;
cout << "Start to compute area" << endl;
double area;
area = findArea(ptA, ptB, ptE);
cout << "The area of triangle ";
ptA.print(); ptB.print(); ptE.print();
cout << " is " << area << endl << endl;
return 0;
}
BOOL CFavUrlMenuDlg::OnEraseBkgnd(CDC* pDC)
{
CRect rctWnd;
GetWindowRect(&rctWnd);
ScreenToClient(rctWnd);
CBrush brush;
brush.CreateSolidBrush(m_rgbBackGnd);
CBrush *pOldBrush = pDC->SelectObject(&brush);
CPen pen;
pen.CreatePen(PS_SOLID, m_nBorderWidth, m_rgbBorder);
CPen *pOldPen = pDC->SelectObject(&pen);
// 画圆角矩形
rctWnd.left += m_nBorderWidth/2;
rctWnd.top += m_nBorderWidth/2;
rctWnd.right -= m_nBorderWidth/2;
rctWnd.bottom -= m_nBorderWidth/2;
if ((m_nBorderWidth % 2) )
{
rctWnd.right -= 1;
rctWnd.bottom -= 1;
}
pDC->RoundRect(rctWnd, CPoint(m_nBorderWidth, m_nBorderWidth));
pDC->SelectObject(pOldPen);
pen.DeleteObject();
pDC->SelectObject(pOldBrush);
brush.DeleteObject();
pDC->SelectObject(m_font);
pDC->SetTextColor(m_rgbText);
pDC->SetBkMode(TRANSPARENT);
CRect rctItem( m_rctFirsItem);
rctItem.left += m_nSpacingHeigth;
for (INT_PTR i=0; i<m_arrItems.GetSize(); i++)
{
ITEM item = m_arrItems.GetAt(i);
if (item.type != ITEM_TYPE_SEPARATOR)
{
pDC->DrawText(item.strName, rctItem, DT_VCENTER|DT_SINGLELINE);
if (item.type == ITEM_TYPE_DIRECTORY)
{ // 画小三角
int y = rctItem.top + rctItem.Height()/2;
int x = rctItem.right - m_nSpacingHeigth;
CPoint ptA(x, y);
y = y - rctItem.Height()/2 + m_nSpacingHeigth/2;
x -= rctItem.Height()/2;
CPoint ptB(x, y);
CPoint ptC(x, y+rctItem.Height() - m_nSpacingHeigth);
CBrush brush;
brush.CreateSolidBrush(m_rgbBorder);
CBrush* pOld = pDC->SelectObject(&brush);
pDC->BeginPath();
pDC->MoveTo(ptA.x,ptA.y);
pDC->LineTo(ptB.x,ptB.y);
pDC->LineTo(ptC.x,ptC.y);
pDC->LineTo(ptA.x,ptA.y);
pDC->EndPath();
pDC->FillPath();
pDC->SelectObject(&pOld);
brush.DeleteObject();
}
}
else
{
CPen pen;
pen.CreatePen(PS_SOLID, 1, m_rgbBorder);
CPen *pOld = pDC->SelectObject(&pen);
int y = rctItem.top + rctItem.Height()/5;
int x = rctItem.left;
rctItem.top += rctItem.Height()/5;
rctItem.top -= rctItem.Height();
LineAtoB(pDC, CPoint(x,y), CPoint(x+rctItem.Width()-m_nSpacingHeigth, y));
pDC->SelectObject(pOld);
pen.DeleteObject();
}
rctItem.top += m_rctFirsItem.Height();
rctItem.bottom = rctItem.top + m_rctFirsItem.Height();
}
pDC->SelectObject(pOldPen);
pen.DeleteObject();
return TRUE;
}
bool AcquireTabletopServer::serverCB(AcquireTabletopRequest& request, AcquireTabletopResponse& response)
{
ROS_INFO("Server Called!");
for(int ii = 0; ii < 3; ii++)
{
for(int jj = 0; jj < 3; jj++)
{
std::cout << (*projection_matrix_)[ii][jj] <<" | ";
}
std::cout << std::endl;
}
float tole;
if(request.tolerance == 0.0 || request.tolerance > 1.0 || request.tolerance < 0.0)
tole = 1.0;
else
tole = request.tolerance;
bool return_value;
std::string object_name = "unknown_object_";
clusters_ptr_.reset();
ros::param::set("/cluster_extraction_enable", true);
while( !clusters_ptr_ && ros::ok())
{
if(clusters_ptr_)
ROS_INFO("We have the clusters.");
usleep(100000);
}
ros::param::set("/cluster_extraction_enable", false);
// Get the image from the rgb/image_raw topic.
image_raw_sub_ = nh_.subscribe("image", 4, &AcquireTabletopServer::imageCallback, this);
while(!image_)
{
//ROS_INFO("Waiting for the camera image...");
usleep(100000);
}
image_raw_sub_.shutdown();
//ROS_INFO("Processing image...");
cv_bridge::CvImagePtr test_image;
sensor_msgs::ImageConstPtr _image_for_processing_ = image_;
test_image = cv_bridge::toCvCopy(_image_for_processing_);
bool pruned = false;
std::vector <std::string> cluster_names;
int unknown_num = 0;
int unseen_num = 0;
tf::StampedTransform camera_to_base_link;
try
{
tf_listener_->waitForTransform(image_->header.frame_id, "ptu_tilt_motor_link", ros::Time(0), ros::Duration(1));
tf_listener_->lookupTransform(image_->header.frame_id, "ptu_tilt_motor_link", ros::Time(0), camera_to_base_link);
}
catch(tf::TransformException& ex)
{
ROS_INFO("COCKED-UP TRANSFORM. ACQUIRE FAILED! Why: %s", ex.what());
return true;
}
for(int i = 0; i < clusters_ptr_->clusters.size(); i++)
{
tf::Vector3 ptA(clusters_ptr_->clusters[i].a.x, clusters_ptr_->clusters[i].a.y, clusters_ptr_->clusters[i].a.z);
tf::Vector3 ptB(clusters_ptr_->clusters[i].b.x, clusters_ptr_->clusters[i].b.y, clusters_ptr_->clusters[i].b.z);
tf::Vector3 ptAInCameraFrame = camera_to_base_link * ptA;
tf::Vector3 ptBInCameraFrame = camera_to_base_link * ptB;
// Now that ptA and ptB are in camera frame, we can project.
tf::Vector3 resultant_pixels_ptA = (*projection_matrix_) * ptAInCameraFrame;
tf::Vector3 resultant_pixels_ptB = (*projection_matrix_) * ptBInCameraFrame;
int start_x = (int) (resultant_pixels_ptA[0]/resultant_pixels_ptA[2] - 70);
int start_y = (int) (resultant_pixels_ptA[1]/resultant_pixels_ptA[2] - 50);
int end_x = (int) (resultant_pixels_ptB[0]/resultant_pixels_ptB[2] - 40);
int end_y = (int) (resultant_pixels_ptB[1]/resultant_pixels_ptB[2] - 30);
if( (start_x < 0 && end_x < 0) || (start_x >= image_width && end_x >= image_width) ||
(start_y < 0 && end_y < 0) || (end_y >= image_height && start_y >= image_height) )
{
//ROS_INFO("UnSeen: %s", cluster_names[i].c_str());
char object_name_with_num[20];
sprintf(object_name_with_num, "unseen_object_%d", unseen_num);
cluster_names.push_back(object_name_with_num);
unseen_num++;
continue;
}
if(start_x < 0)
start_x = 0;
if(start_y < 0)
start_y = 0;
if(end_x >= image_width)
end_x = image_width - 1;
if(end_y >= image_height)
end_y = image_height - 1;
//ROS_INFO("Widths: (%d,%d) and (%d,%d)", cam_s_x, cam_s_y, cam_e_x, cam_e_y);
cv::Mat new_part = cutImage(test_image->image, start_y, start_x, end_y-start_y, end_x-start_x);
//ROS_INFO("SHOWING CUT IMAGE: SIZE: %d, %d", end_x-start_x, end_y-start_y);
//cv::imshow("sucks", new_part);
//cv::waitKey(0);
std::string name_from_sift = processImage(new_part, tole);
if(name_from_sift.find("unknown_object") != std::string::npos)
{
char object_number[4];
sprintf(object_number, "%d", unknown_num);
name_from_sift += object_number;
unknown_num++;
}
cluster_names.push_back(name_from_sift);
//ROS_INFO("Seen: %s", cluster_names[i].c_str());
}
if(clusters_ptr_->clusters.size() != 0)
response.objects.header.frame_id = clusters_ptr_->clusters[0].centroid.header.frame_id;
// Before we Anchor, let's retrive the signatures from CAM. //
cam_objects_ = cam_interface::getAllObjectSignaturesFromCAM(tf_listener_);
for(int i = 0; i < clusters_ptr_->clusters.size(); i++)
{
doro_msgs::TableObject __object;
__object.centroid = clusters_ptr_->clusters[i].centroid.point;
__object.cluster_size = clusters_ptr_->clusters[i].cluster_size;
__object.color = clusters_ptr_->clusters[i].color;
/* ********************************************************************** *
* This is the part where we anchor! We set the id (symbol) to a value by *
* using the sift matching. If sift can't find it, we use other signature *
* values and compare it to the features here. *
* ********************************************************************** */
// Stage 1: Using sift recognized id.
__object.id = cluster_names[i];
// Stage 2: Using signatures from CAM.
if(cluster_names[i].find("unseen") != std::string::npos ||
cluster_names[i].find("unknown") != std::string::npos ||
cluster_names[i].empty())
{
//anchorUsingSignature(__object, tole);
}
response.objects.table_objects.push_back(__object);
}
// Pruning out.
response.objects.number_of_objects = response.objects.table_objects.size();
if(request.signature.cluster_size.size() != 0)
{
for(std::vector<doro_msgs::TableObject>::iterator iter_obj = response.objects.table_objects.begin();
iter_obj != response.objects.table_objects.end();
iter_obj++)
{
if(iter_obj->cluster_size[0] > (0.025 + request.signature.cluster_size[0]) ||
iter_obj->cluster_size[0] < (request.signature.cluster_size[0] - 0.025) ||
iter_obj->cluster_size[1] > (0.025 + request.signature.cluster_size[1]) ||
iter_obj->cluster_size[1] < (request.signature.cluster_size[1] - 0.025) ||
iter_obj->cluster_size[2] > (0.025 + request.signature.cluster_size[2]) ||
iter_obj->cluster_size[2] < (request.signature.cluster_size[2] - 0.025) )
{
// If the size is out of bounds, remove it.
response.objects.table_objects.erase(iter_obj);
iter_obj--;
}
}
ROS_INFO("We pruned for size. There are %d objects now.", response.objects.table_objects.size());
pruned = true;
}
else
ROS_INFO("Size Ignored.");
if(request.signature.color.size() == 3)
{
//int color = (request.signature.color[0] << 16) | (request.signature.color[1] << 8) | (request.signature.color[2]);
for(std::vector<doro_msgs::TableObject>::iterator iter_obj = response.objects.table_objects.begin();
iter_obj != response.objects.table_objects.end();
iter_obj++)
{
if( (iter_obj->color[0] > (50 + request.signature.color[0]) || iter_obj->color[0] < (request.signature.color[0] - 50) ) ||
(iter_obj->color[1] > (50 + request.signature.color[1]) || iter_obj->color[1] < (request.signature.color[1] - 50) ) ||
(iter_obj->color[2] > (50 + request.signature.color[2]) || iter_obj->color[2] < (request.signature.color[2] - 50) ) )
{
response.objects.table_objects.erase(iter_obj);
iter_obj--;
}
}
ROS_INFO("We pruned for color. There are %d objects now.", response.objects.table_objects.size());
pruned = true;
}
else
ROS_INFO("Color Ignored.");
// In this case return only the element requested.
// That is, we return only that element which has the same id as in the signature.
for(std::vector<doro_msgs::TableObject>::iterator iter_obj = response.objects.table_objects.begin();
iter_obj != response.objects.table_objects.end();
iter_obj++)
{
if(request.type == request.KNOWN && (iter_obj->id.empty() || iter_obj->id.find("unknown") != std::string::npos || iter_obj->id.find("unseen") != std::string::npos) )
{
response.objects.table_objects.erase(iter_obj);
iter_obj--;
continue;
}
else if(request.type == request.NAME &&
iter_obj->id.compare(request.signature.id) != 0)
{
//ROS_INFO("%s != %s", iter_obj->id.c_str(), request.signature.id.c_str());
response.objects.table_objects.erase(iter_obj);
iter_obj--;
continue;
}
}
return_value = true;
ROS_INFO("****************");
ROS_INFO("* Response set *");
ROS_INFO("****************");
clusters_ptr_.reset();
image_.reset();
return return_value;
}
