Skip to content

Commit

Permalink
hacked rendering process. POsePlanner displays the belief state durin…
Browse files Browse the repository at this point in the history 
…g the execution of the demo.
  • Loading branch information
@memnone
memnone committed Jun 23, 2016
1 parent c435ceb commit 77423c7
Show file tree
Hide file tree
Showing 4 changed files with 81 additions and 21 deletions.
15 changes: 15 additions & 0 deletions include/Spam/App/PosePlanner/PosePlanner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -274,6 +274,13 @@ class PosePlanner : public grasp::Player {
/** Manipulator Appearance */
grasp::Manipulator::Appearance manipulatorAppearance;

/** Appereance for point clouds: hypothesis point clouds */
grasp::Cloud::Appearance hypothesisAppearance;
/** Appereance for point clouds: debug point clouds */
grasp::Cloud::Appearance meanposeAppearance;
/** Appereance for point clouds: ground truth point clouds */
grasp::Cloud::Appearance groundTruthAppearance;

/** Constructs from description object */
Desc() {
Desc::setToDefault();
Expand Down Expand Up @@ -334,6 +341,10 @@ class PosePlanner : public grasp::Player {

manipulatorDesc.reset(new grasp::Manipulator::Desc);
manipulatorAppearance.setToDefault();

hypothesisAppearance.setToDefault();
meanposeAppearance.setToDefault();
groundTruthAppearance.setToDefault();
}
/** Checks if the description is valid. */
virtual void assertValid(const grasp::Assert::Context& ac) const {
Expand Down Expand Up @@ -364,6 +375,10 @@ class PosePlanner : public grasp::Player {
/** Descriptor file */
Desc myDesc;

/** Appereance for point clouds: debug point clouds */
grasp::Cloud::Appearance hypothesisAppearance, meanposeAppeareance, groundtruthAppearance;
bool showMeanPoseOnly, showSimulate;

/** Force to draw the belief state */
bool drawBeliefState;
/** Pointer to the current belief state */
Expand Down
17 changes: 17 additions & 0 deletions resources/Spam/Demo/FT/SpamDemoFT_RobotBorisSim.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,23 @@
</query>
</appearance>

<meanpose_appearance show_frame="1" show_points="1" mode="point" mode_3d="1" mode_3d_a="177" point_size="1.0" frame_num="100" camera_frame="1" camera_frame_size="0.05" feature_curv_pow="1.0">
<frame_size v1="0.1" v2="0.1" v3="0.1"/>
<!-- GREEN -->
<colour override="1" R="0" G="255" B="0" A="255"/>
</meanpose_appearance>
<hypothesis_appearance show_frame="1" show_points="1" mode="point" mode_3d="1" mode_3d_a="177" point_size="1.0" frame_num="100" camera_frame="1" camera_frame_size="0.05" feature_curv_pow="1.0">
<frame_size v1="0.01" v2="0.01" v3="0.01"/>
<!-- BLUE -->
<colour override="1" R="0" G="0" B="255" A="255"/>
</hypothesis_appearance>
<groundtruth_appearance show_frame="0" show_points="1" mode="point" mode_3d="1" mode_3d_a="177" point_size="1.0" frame_num="100" camera_frame="1" camera_frame_size="0.05" feature_curv_pow="1.0">
<frame_size v1="0.1" v2="0.1" v3="0.1"/>
<!-- BLACK -->
<colour override="1" R="0" G="0" B="0" A="255"/>
</groundtruth_appearance>


<pose_estimation_sim item="ground_truth_pose" handler="PointsCurv+PointsCurv" points="100000" features="500000" attempts="10" kernels="50000" neighbours="20000" distance_range="20.0" distance_max="0.02" feature_norm_eps="1e-7" search_checks="32" search_kdtrees="4">
<nn_search neighbours="0" search_checks="32" search_kdtrees="4" search_leaf_max_size="10"/>
<covariance dim="7" c1="0.01" c2="0.01" c3="0.01" c4="0.1" c5="0.1" c6="0.1" c7="0.1"/>
Expand Down
69 changes: 49 additions & 20 deletions src/Spam/App/PosePlanner/PosePlanner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,15 +181,32 @@ void spam::PosePlanner::Data::createRender() {
Player::Data::createRender();
{
golem::CriticalSectionWrapper csw(owner->getCS());
// show constantly the belief state, if needed
const grasp::data::Item::Map::iterator ptr = itemMap.find(owner->currentBeliefItem);
if (owner->drawBeliefState && ptr != itemMap.end()) {
//ptr->second->createRender();
data::ItemBelief* pItem = grasp::to<data::ItemBelief>(ptr);
if (pItem)
owner->addRenderer(is<golem::UIRenderer>(&pItem->getHandler()));
//pItem->customRender();

// draw ground truth
if (!simulateObjectPose.empty() && owner->showSimulate) {
owner->groundtruthAppearance.draw(simulateObjectPose, owner->debugRenderer);
}

// draw hypothesis
if (owner->pBelief.get() && owner->drawBeliefState) {
for (Hypothesis::Seq::const_iterator i = owner->pBelief->getHypotheses().begin(); i != owner->pBelief->getHypotheses().end(); ++i) {
grasp::Cloud::Appearance& appearance = i == owner->pBelief->getHypotheses().begin() ? owner->meanposeAppeareance : owner->hypothesisAppearance;
owner->debugRenderer.addAxes((*i)->toRBPoseSampleGF().toMat34(), appearance.frameSize);
appearance.draw((*i)->getCloud(), owner->debugRenderer);
if (owner->showMeanPoseOnly) // this parameter can be control by outside
break;
}
}

// show constantly the belief state, if needed
//const grasp::data::Item::Map::iterator ptr = itemMap.find(owner->currentBeliefItem);
//if (owner->drawBeliefState && ptr != itemMap.end()) {
// //ptr->second->createRender();
// data::ItemBelief* pItem = grasp::to<data::ItemBelief>(ptr);
// if (pItem)
// owner->addRenderer(is<golem::UIRenderer>(&pItem->getHandler()));
// //pItem->customRender();
//}
}
}

Expand Down Expand Up @@ -278,6 +295,13 @@ void PosePlanner::Desc::load(golem::Context& context, const golem::XMLContext* x

manipulatorDesc->load(xmlcontext->getContextFirst("manipulator"));
manipulatorAppearance.load(xmlcontext->getContextFirst("manipulator appearance"));

try {
hypothesisAppearance.xmlData(xmlcontext->getContextFirst("hypothesis_appearance", false), false);
meanposeAppearance.xmlData(xmlcontext->getContextFirst("meanpose_appearance", false), false);
groundTruthAppearance.xmlData(xmlcontext->getContextFirst("groundtruth_appearance", false), false);
}
catch (const golem::MsgXMLParser& msg) { context.write("%s\n", msg.str().c_str()); }
}

//------------------------------------------------------------------------------
Expand All @@ -289,7 +313,7 @@ const std::string PosePlanner::Data::ModeName[MODE_QUERY + 1] = {

//------------------------------------------------------------------------------

spam::PosePlanner::PosePlanner(Scene &scene) : grasp::Player(scene), rand(context.getRandSeed()), pHeuristic(nullptr), drawBeliefState(false), currentBeliefItem() {
spam::PosePlanner::PosePlanner(Scene &scene) : grasp::Player(scene), rand(context.getRandSeed()), pHeuristic(nullptr), drawBeliefState(false), showMeanPoseOnly(false), showSimulate(true), currentBeliefItem() {
}

bool spam::PosePlanner::create(const Desc& desc) {
Expand Down Expand Up @@ -395,6 +419,9 @@ bool spam::PosePlanner::create(const Desc& desc) {
modelFrame.setId();

queryViews = 3;
hypothesisAppearance = desc.hypothesisAppearance;
meanposeAppeareance = desc.meanposeAppearance;
groundtruthAppearance = desc.groundTruthAppearance;

myDesc = desc;

Expand Down Expand Up @@ -531,17 +558,19 @@ void spam::PosePlanner::render() const {
//------------------------------------------------------------------------------

void spam::PosePlanner::resetDataPointers() {
const grasp::data::Item::Map::iterator ptr = to<Data>(dataCurrentPtr)->itemMap.find(currentBeliefItem);
if (ptr != to<Data>(dataCurrentPtr)->itemMap.end()) {
//ptr->second->createRender();
data::ItemBelief* pItem = grasp::to<data::ItemBelief>(ptr);
if (pItem) {
pItem->showMeanPoseOnly(true);
pItem->showQuery(false);
pItem->showGroundTruth(true);
}
}

showMeanPoseOnly = false;
showSimulate = true;

//const grasp::data::Item::Map::iterator ptr = to<Data>(dataCurrentPtr)->itemMap.find(currentBeliefItem);
//if (ptr != to<Data>(dataCurrentPtr)->itemMap.end()) {
// //ptr->second->createRender();
// data::ItemBelief* pItem = grasp::to<data::ItemBelief>(ptr);
// if (pItem) {
// pItem->showMeanPoseOnly(true);
// pItem->showQuery(false);
// pItem->showGroundTruth(true);
// }
//}
}

//------------------------------------------------------------------------------
Expand Down
1 change: 0 additions & 1 deletion src/Spam/Demo/FT/FTDemo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -337,7 +337,6 @@ void FTDemo::create(const Desc& desc) {
golem::CriticalSectionWrapper cswData(getCS());
to<Data>(dataCurrentPtr)->itemMap.erase(queryItemTrj);
queryTrjPtr = to<Data>(dataCurrentPtr)->itemMap.insert(to<Data>(dataCurrentPtr)->itemMap.end(), grasp::data::Item::Map::value_type(queryItemTrj, queryGraspTrj));
Data::View::setItem(to<Data>(dataCurrentPtr)->itemMap, currentBeliefPtr, to<Data>(dataCurrentPtr)->getView());
//Data::View::setItem(to<Data>(dataCurrentPtr)->itemMap, queryTrjPtr, to<Data>(dataCurrentPtr)->getView());
}
to<Data>(dataCurrentPtr)->createRender();
Expand Down

0 comments on commit 77423c7

Please sign in to comment.