std::unique_ptr<output> preprocess(std::unique_ptr<input> in)
{
cv::Mat greyscale = cv::imread(config->input_filename(), CV_LOAD_IMAGE_GRAYSCALE);
cv::medianBlur(greyscale, greyscale, 9);
std::string workspace = config->workspace_path();
cv::imwrite(workspace + "smooth.pgm", greyscale);
{
// because OpenCV sucks at saving pbm
// or I suck at OpenCV...
qgar::PgmFile pgm((workspace + "smooth.pgm").c_str());
qgar::PbmFile pbm((workspace + "smooth.pbm").c_str());
pbm.write(qgar::ThresBinaryImage(pgm.read(), 170));
}
try
{
executor::os_proxy().call("QAtextGraphicSeparation.exe", { "-in", workspace + "smooth.pbm", "-outxt", workspace + "text.pbm", "-outg",
workspace + "graphics.pbm", "-outu", workspace + "unknown.pbm", "-area", "10", "-elbbox", "100", "-elmaer", "2", "-dmaer", ".5" });
}
catch (executor::system_call_failure c)
{
config->log().error_log(std::string("System call failure: ") + c.what() + "\nContinuing, but expect crashes");
}
auto skeleton = skeletize_zhang_suen(greyscale);
cv::imwrite(config->workspace_path() + "whole_skeleton.pgm", skeleton);
auto wide_lines = pick_wide_lines(workspace + "graphics.pbm");
cv::imwrite(config->workspace_path() + "wide_lines_only.pgm", wide_lines);
auto wide_skeleton = skeletize_zhang_suen(wide_lines);
cv::imwrite(config->workspace_path() + "wide_lines_only_skeleton.pgm", wide_skeleton);
std::unique_ptr<output> out(std::make_unique<output>());
out->wide_lines_filename = config->workspace_path() + "wide_lines_only_skeleton.pgm";
out->original_filename = config->workspace_path() + "smooth.pgm";
return out;
}
void test_PositionBasedManipulator()
{
xde::sys::Timer timer;
xde::builder::SimpleViewer viewer("../../../../share/resources/ogre");
OGREViewer::SceneInterface& si = viewer.getSceneInterface();
OGREViewer::CameraInterface& ci = viewer.getCameraInterface();
std::cout << "Creating viewer: " << timer.GetTime() * .001 << " sec." << std::endl;
timer.ResetTime();
xde::gvm::SceneRef scene = xde::gvm::SceneRef::createObject("main");
scene.setIntegratorFlags(xde::gvm::DYNAMIC_INTEGRATOR | xde::gvm::GAUSS_SEIDEL_SOLVER | xde::gvm::USE_NON_LINEAR_TERMS);
scene.setUcRelaxationFactor(.1);
scene.setFdvrFactor(.2);
scene.setTimeStep(.01);
xde::xcd::SceneRef xcd = xde::lmd::SceneRef::createObject("lmd", .02, .05);
scene.setGeometricalScene(xcd);
std::cout << "Creating simulation: " << timer.GetTime() * .001 << " sec." << std::endl;
timer.ResetTime();
xde::data::Assets assets;
assets.importOpenColladaFile("../../../../share/resources/scenes/ground.dae", Eigen::Vector3d::Ones());
assets.importOpenColladaFile("../../../../share/resources/scenes/knob.dae", Eigen::Vector3d::Constant(.2));
std::cout << "Parsing data: " << timer.GetTime() * .001 << " sec." << std::endl;
timer.ResetTime();
xde::builder::GraphicsBuilder graphBuilder(viewer.getSceneComponents(), &assets);
graphBuilder.loadAssets();
const std::string& knobId = assets.getIds().getLastId("../../../../share/resources/scenes/knob.dae", "knob");
graphBuilder.createNodeFromTree("knob", "", knobId, knobId, false);
graphBuilder.createNodeFromTree("knobProxy", "", knobId, knobId, false);
si.setNodeMaterial("knobProxy", "xde/OrangeOpaque");
const std::string& groundId = assets.getIds().getLastId("../../../../share/resources/scenes/ground.dae", "node-ground");
graphBuilder.createNodeFromTree("ground", "", groundId, groundId, false);
std::cout << "Building graphics: " << timer.GetTime() * .001 << " sec." << std::endl;
xde::builder::PhysicsBuilder phyBuilder(xcd, &assets);
timer.ResetTime();
xde::xcd::CompositeRef groundComposite = phyBuilder.createCompositeFromTree("ground.comp", groundId, groundId, .005, 1., false, false);
xde::xcd::CompositeRef knobComposite = phyBuilder.createCompositeFromTree("knob.comp", knobId, knobId, .005, .1, false, false);
xde::xcd::CompositeRef knobProxyComposite = phyBuilder.createCompositeFromTree("knobProxy.comp", knobId, knobId, .005, .1, false, false);
std::cout << "Building collision: " << timer.GetTime() * .001 << " sec." << std::endl;
timer.ResetTime();
xde::gvm::RigidBodyRef groundBody = xde::gvm::RigidBodyRef::createObject("ground");
xde::gvm::FixedJointRef groundJoint = xde::gvm::FixedJointRef::createObject("ground.joint");
scene.addRigidBodyToGround(groundBody, groundJoint);
groundBody.setComposite(groundComposite);
groundJoint.configure(Eigen::Displacementd::Identity());
xde::gvm::RigidBodyRef knobBody = xde::gvm::RigidBodyRef::createObject("knob");
xde::gvm::FreeJointRef knobJoint = xde::gvm::FreeJointRef::createObject("knob.joint");
scene.addRigidBodyToGround(knobBody, knobJoint);
knobBody.setComposite(knobComposite);
knobBody.setMass(1.);
knobBody.computePrincipalFrameAndMomentsOfInertiaUsingCompositeObbAndMass();
knobJoint.configure(Eigen::Displacementd(0., 0., 1., .707, 0., .707, 0.));
xde::gvm::RigidBodyRef knobProxyBody = xde::gvm::RigidBodyRef::createObject("knobProxy");
xde::gvm::FreeJointRef knobProxyJoint = xde::gvm::FreeJointRef::createObject("knobProxy.joint");
scene.addRigidBodyToGround(knobProxyBody, knobProxyJoint);
knobProxyBody.setComposite(knobProxyComposite);
knobProxyBody.setMass(1.);
knobProxyBody.computePrincipalFrameAndMomentsOfInertiaUsingCompositeObbAndMass();
knobProxyJoint.configure(Eigen::Displacementd(0., 0., 1., .707, 0., .707, 0.));
scene.enableContactForBodyPair(knobBody, groundBody, true);
std::cout << "Building physics: " << timer.GetTime() * .001 << " sec." << std::endl;
timer.ResetTime();
/*std::vector<xde::hardware::SpaceMouseDesc> smds = xde::hardware::SpaceMouse::scan();
xde::hardware::SpaceMouse sm(smds[0]);
sm.setMaxLinearVelocity(1.);
sm.setMaxAngularVelocity(3.14);
sm.setDominatingModeOn();*/
xde::manip::VelocityBasedManipulator vbm(scene);
vbm.setTau(.01);
vbm.attach(knobProxyBody);
knobProxyBody.disableWeight();
std::cout << "Building spacemouse: " << timer.GetTime() * .001 << " sec." << std::endl;
timer.ResetTime();
xde::manip::PositionBasedManipulator pbm(scene);
pbm.setTau(.01);
pbm.mapBaseFrameToObservationFrame();
pbm.setInputPosition(Eigen::Displacementd::Identity(), 1., 1.);
pbm.attach(knobBody);
knobBody.disableWeight();
std::cout << "Building pseudo art: " << timer.GetTime() * .001 << " sec." << std::endl;
for(int i = 0; i < 5000; ++i)
{
const double t = i * .01;
const double w = 2 * M_PI * 1.;
Eigen::Displacementd H_ref(1.5 * std::sin(w * t), 0., 0., 1., 0., 0., 0.);
viewer.update();
//sm.update();
vbm.setObservationFrame(ci.getCameraDisplacement(ci.getMainCameraName()));
//vbm.setInputVelocity(sm.getVelocity());
vbm.preUpdate();
pbm.setObservationFrame(ci.getCameraDisplacement(ci.getMainCameraName()));
if((i < 500) || (i > 675))
pbm.setInputPosition(H_ref, 1., 1.);
pbm.preUpdate();
scene.detectCollisions();
scene.integrate();
vbm.postUpdate();
pbm.postUpdate();
si.setNodePosition("ground", groundBody.getPosition());
si.setNodePosition("knob", knobBody.getPosition());
si.setNodePosition("knobProxy", knobProxyBody.getPosition());
xde::sys::Timer::Sleep(10);
}
pbm.detach();
vbm.detach();
scene.removeRigidBody(groundBody);
scene.removeRigidBody(knobBody);
scene.removeRigidBody(knobProxyBody);
scene.printPerformanceReport(std::cout);
system("PAUSE");
}
