void Tessellate(
NodeVector & vecNodes,
FaceVector & vecFaces
) {
int nInitialNodeListSize = vecNodes.size();
// Create centerpoint nodes
for (int i = 0; i < vecFaces.size(); i++) {
InsertQuadNodeCenter(
vecNodes,
vecNodes[vecFaces[i][0]],
vecNodes[vecFaces[i][1]],
vecNodes[vecFaces[i][2]],
vecNodes[vecFaces[i][3]]);
}
// Construct tesselation
SegmentMap mapSegment;
ConstructSegmentMap(vecFaces, mapSegment, -1);
vecFaces.clear();
SegmentMapIterator iter = mapSegment.begin();
for (; iter != mapSegment.end(); iter++) {
Face faceNew =
Face(
iter->first[1],
nInitialNodeListSize + iter->second[0],
iter->first[0],
nInitialNodeListSize + iter->second[1]);
vecFaces.push_back(faceNew);
}
}
void LodExtract::writeHomologies(const Genome* inParent,
const vector<const Genome*>& inChildren)
{
vector<const Genome*> inGenomes = inChildren;
inGenomes.push_back(inParent);
Genome* outParent = _outAlignment->openGenome(inParent->getName());
assert(outParent != NULL && outParent->getNumBottomSegments() > 0);
assert(inChildren.size() > 0);
Genome* outChild = _outAlignment->openGenome(inChildren[0]->getName());
BottomSegmentIteratorPtr bottom = outParent->getBottomSegmentIterator();
TopSegmentIteratorPtr top = outChild->getTopSegmentIterator();
// FOR EVERY BLOCK
for (hal_size_t blockIdx = 0; blockIdx < _graph.getNumBlocks(); ++blockIdx)
{
SegmentMap segMap;
const LodBlock* block = _graph.getBlock(blockIdx);
for (hal_size_t segIdx = 0; segIdx < block->getNumSegments(); ++segIdx)
{
const LodSegment* segment = block->getSegment(segIdx);
const Genome* genome = segment->getSequence()->getGenome();
// ADD TO MAP
pair<SegmentMap::iterator, bool> res = segMap.insert(
pair<const Genome*, SegmentSet*>(genome, NULL));
if (res.second == true)
{
assert(res.first->second == NULL);
res.first->second = new SegmentSet();
}
res.first->second->insert(segment);
}
updateBlockEdges(inParent, segMap, block, bottom, top);
// free the temporary sets!
for (SegmentMap::iterator mapIt = segMap.begin(); mapIt != segMap.end();
++mapIt)
{
delete mapIt->second;
}
}
}
double computeMass(const Tree & tree) {
double total_mass = 0.0;
//create necessary vectors
SegmentMap::const_iterator root;
root = tree.getRootSegment();
SegmentMap sm = tree.getSegments();
for( SegmentMap::const_iterator i=sm.begin(); i!=sm.end(); ++i ) {
//root has no mass
if( i != root ) {
total_mass += GetTreeElementSegment(i->second).getInertia().getMass();
}
}
return total_mass;
}
void plStealthDlgProc::ILoadLoops(IParamBlock2 *pb)
{
HWND hLoops = GetDlgItem( fhWnd, IDC_LOOPS );
SendMessage( hLoops, CB_RESETCONTENT, 0, 0 );
// Add the default option
int defIdx = SendMessage( hLoops, CB_ADDSTRING, 0, (LPARAM)ENTIRE_ANIMATION_NAME );
SendMessage( hLoops, CB_SETITEMDATA, defIdx, kDefault );
plString segName = plString::FromUtf8( pb->GetStr( (ParamID)plAnimStealthNode::kPBName ) );
if( segName.IsNull() || fSegMap == nil )
{
// Default of "entire animation", no other loop options
SendMessage( hLoops, CB_SETCURSEL, defIdx, 0 );
return;
}
SegmentSpec *animSpec = (*fSegMap)[ segName ];
if( animSpec && fSegMap )
{
// for each segment we found:
for( SegmentMap::iterator i = fSegMap->begin(); i != fSegMap->end(); i++ )
{
SegmentSpec *spec = (*i).second;
if( spec->fType == SegmentSpec::kLoop )
{
// If the loop is contained by the animation, add it
if( (spec->fStart == -1 || spec->fStart >= animSpec->fStart) &&
(spec->fEnd == -1 || spec->fEnd <= animSpec->fEnd) )
{
// Add the name
int idx = SendMessage( hLoops, CB_ADDSTRING, 0, (LPARAM)spec->fName.c_str() );
SendMessage( hLoops, CB_SETITEMDATA, idx, kName );
}
}
}
}
ISetSel( hLoops, pb->GetStr( (ParamID)plAnimStealthNode::kPBLoopName ) );
}
SegmentMap *GetSharedAnimSegmentMap(std::vector<Animatable*>& anims, plErrorMsg *pErrorMsg)
{
if (anims.empty())
return nil;
SegmentMap *segMap = GetAnimSegmentMap(anims[0], pErrorMsg);
if (!segMap)
return nil;
int i;
for (i = 1; i < anims.size(); i++)
{
SegmentMap *curSegMap = GetAnimSegmentMap(anims[i], pErrorMsg);
// This node doesn't have a segmap, so we can't have any anims shared among all the nodes.
if (!curSegMap)
{
DeleteSegmentMap(segMap);
return nil;
}
if (segMap->begin() == segMap->end())
{
DeleteSegmentMap(segMap);
return nil;
}
SegmentMap::iterator it = segMap->begin();
while (it != segMap->end())
{
if (curSegMap->find(it->second->fName) == curSegMap->end())
{
SegmentMap::iterator del = it;
it++;
segMap->erase(del->second->fName);
}
else
it++;
}
DeleteSegmentMap(curSegMap);
}
return segMap;
}
void LodExtract::updateBlockEdges(const Genome* inParentGenome,
SegmentMap& segMap,
const LodBlock* block,
BottomSegmentIteratorPtr bottom,
TopSegmentIteratorPtr top)
{
Genome* outParentGenome = bottom->getGenome();
const LodSegment* rootSeg = NULL;
SegmentSet* segSet;
SegmentSet::iterator setIt;
// Zap all segments in parent genome
SegmentMap::iterator mapIt = segMap.find(inParentGenome);
if (mapIt != segMap.end())
{
segSet = mapIt->second;
assert(segSet != NULL);
setIt = segSet->begin();
for (; setIt != segSet->end(); ++setIt)
{
bottom->setArrayIndex(outParentGenome, (*setIt)->getArrayIndex());
for (hal_size_t i = 0; i < bottom->getNumChildren(); ++i)
{
bottom->setChildIndex(i, NULL_INDEX);
bottom->setTopParseIndex(NULL_INDEX);
}
}
// Choose first segment as parent to all segments in the child genome
setIt = segSet->begin();
rootSeg = *(setIt);
bottom->setArrayIndex(outParentGenome, (*setIt)->getArrayIndex());
}
// Do the child genomes
const Genome* inGrandParentGenome = inParentGenome->getParent();
SegmentSet::iterator nextIt;
for (mapIt = segMap.begin(); mapIt != segMap.end(); ++mapIt)
{
if (mapIt->first != inParentGenome and mapIt->first != inGrandParentGenome)
{
Genome* outChildGenome =
_outAlignment->openGenome(mapIt->first->getName());
hal_index_t childIndex = outParentGenome->getChildIndex(outChildGenome);
assert(childIndex >= 0);
segSet = mapIt->second;
assert(segSet != NULL);
for (setIt = segSet->begin(); setIt != segSet->end(); ++setIt)
{
top->setArrayIndex(outChildGenome, (*setIt)->getArrayIndex());
top->setBottomParseIndex(NULL_INDEX);
// Connect to parent
if (rootSeg != NULL)
{
top->setParentIndex(bottom->getArrayIndex());
bool reversed = (*setIt)->getFlipped() != rootSeg->getFlipped();
top->setParentReversed(reversed);
if (setIt == segSet->begin())
{
bottom->setChildIndex(childIndex, top->getArrayIndex());
bottom->setChildReversed(childIndex, reversed);
}
}
else
{
top->setParentIndex(NULL_INDEX);
}
// Connect to next paralogy
SegmentSet::iterator setNext = setIt;
++setNext;
if (setNext == segSet->end())
{
setNext = segSet->begin();
}
if (setNext == setIt)
{
top->setNextParalogyIndex(NULL_INDEX);
}
else
{
top->setNextParalogyIndex((*setNext)->getArrayIndex());
}
}
}
}
}
/**
* Request the root segment of the tree
*
* @return constant iterator pointing to the root segment
*/
SegmentMap::const_iterator getRootSegment()const
{
return segments.find(root_name);
};
/**
* Request the segment of the tree with name segment_name.
*
* @param segment_name the name of the requested segment
*
* @return constant iterator pointing to the requested segment
*/
SegmentMap::const_iterator getSegment(const std::string& segment_name)const
{
return segments.find(segment_name);
};
void RefineEverything(
NodeVector & vecNodes,
FaceVector & vecFaces,
int nResolution
) {
// Generate segment map
SegmentMap mapSegment;
ConstructSegmentMap(vecFaces, mapSegment, -1);
FaceVector vecFacesOld = vecFaces;
// Loop over all faces
vecFaces.clear();
// Construct map from segments to edges
std::map<Segment, Edge> mapEdge;
SegmentMapIterator iter = mapSegment.begin();
for (; iter != mapSegment.end(); iter++) {
Edge edge;
GenerateEdgeVertices(
nResolution,
iter->first[0],
iter->first[1],
vecNodes,
edge);
mapEdge.insert(std::pair<Segment, Edge>(iter->first, edge));
}
// Loop over all faces and refine
for (int n = 0 ; n < vecFacesOld.size(); n++) {
const Segment & seg0 = vecFacesOld[n].iterSegment[0]->first;
const Segment & seg1 = vecFacesOld[n].iterSegment[1]->first;
const Segment & seg2 = vecFacesOld[n].iterSegment[2]->first;
const Segment & seg3 = vecFacesOld[n].iterSegment[3]->first;
Edge edge0 = mapEdge.find(seg0)->second;
Edge edge1 = mapEdge.find(seg1)->second;
Edge edge3 = mapEdge.find(seg2)->second;
Edge edge2 = mapEdge.find(seg3)->second;
// Align bottom and left edge
if (edge0[0] == edge1[0]) {
} else if (edge0[0] == edge1[edge1.size()-1]) {
edge1 = edge1.Flip();
} else if (edge0[edge0.size()-1] == edge1[0]) {
edge0 = edge0.Flip();
} else if (edge0[edge0.size()-1] == edge1[edge1.size()-1]) {
edge0 = edge0.Flip();
edge1 = edge1.Flip();
} else {
_EXCEPTIONT("Logic error");
}
// Align bottom and right edge
if (edge0[edge0.size()-1] == edge2[0]) {
} else if (edge0[edge0.size()-1] == edge2[edge2.size()-1]) {
edge2 = edge2.Flip();
} else {
_EXCEPTIONT("Logic error");
}
// Align top and left edge
if (edge1[edge1.size()-1] == edge3[0]) {
} else if (edge1[edge1.size()-1] == edge3[edge3.size()-1]) {
edge3 = edge3.Flip();
} else {
_EXCEPTIONT("Logic error");
}
Edge edgeTop;
Edge edgeBot = edge0;
for (int j = 0; j < nResolution; j++) {
// Generate top level edge
if (j != nResolution-1) {
int ix0 = edge1[j+1];
int ix1 = edge2[j+1];
GenerateEdgeVertices(nResolution, ix0, ix1, vecNodes, edgeTop);
} else {
edgeTop = edge3;
}
// Generate face
for (int i = 0; i < nResolution; i++) {
Face face(
edgeBot[i+1], edgeBot[i],
edgeTop[i], edgeTop[i+1],
vecFacesOld[n].nRefineLevel);
face.nColor = vecFacesOld[n].nColor;
face.nTag = vecFacesOld[n].nTag;
vecFaces.push_back(face);
}
// Increment row
edgeBot = edgeTop;
}
}
}
bool calibrateSegmentCallback(vicon_bridge::viconCalibrateSegment::Request& req,
vicon_bridge::viconCalibrateSegment::Response& resp)
{
std::string full_name = req.subject_name + "/" + req.segment_name;
ROS_INFO("trying to calibrate %s", full_name.c_str());
SegmentMap::iterator seg_it = segment_publishers_.find(full_name);
if (seg_it == segment_publishers_.end())
{
ROS_WARN("frame %s not found --> not calibrating", full_name.c_str());
resp.success = false;
resp.status = "segment " + full_name + " not found";
return false;
}
SegmentPublisher & seg = seg_it->second;
if (seg.calibrated)
{
ROS_INFO("%s already calibrated, deleting old calibration", full_name.c_str());
seg.calibration_pose.setIdentity();
}
vicon_bridge::viconGrabPose::Request grab_req;
vicon_bridge::viconGrabPose::Response grab_resp;
grab_req.n_measurements = req.n_measurements;
grab_req.subject_name = req.subject_name;
grab_req.segment_name = req.segment_name;
bool ret = grabPoseCallback(grab_req, grab_resp);
if (!ret)
{
resp.success = false;
resp.status = "error while grabbing pose from Vicon";
return false;
}
tf::Transform t;
t.setOrigin(tf::Vector3(grab_resp.pose.pose.position.x, grab_resp.pose.pose.position.y,
grab_resp.pose.pose.position.z - req.z_offset));
t.setRotation(tf::Quaternion(grab_resp.pose.pose.orientation.x, grab_resp.pose.pose.orientation.y,
grab_resp.pose.pose.orientation.z, grab_resp.pose.pose.orientation.w));
seg.calibration_pose = t.inverse();
// write zero_pose to parameter server
string param_suffix(full_name + "/zero_pose/");
nh_priv.setParam(param_suffix + "orientation/w", t.getRotation().w());
nh_priv.setParam(param_suffix + "orientation/x", t.getRotation().x());
nh_priv.setParam(param_suffix + "orientation/y", t.getRotation().y());
nh_priv.setParam(param_suffix + "orientation/z", t.getRotation().z());
nh_priv.setParam(param_suffix + "position/x", t.getOrigin().x());
nh_priv.setParam(param_suffix + "position/y", t.getOrigin().y());
nh_priv.setParam(param_suffix + "position/z", t.getOrigin().z());
ROS_INFO_STREAM("calibration completed");
resp.pose = grab_resp.pose;
resp.success = true;
resp.status = "calibration successful";
seg.calibrated = true;
return true;
}
void process_subjects(const ros::Time& frame_time)
{
string tracked_frame, subject_name, segment_name;
unsigned int n_subjects = msvcbridge::GetSubjectCount().SubjectCount;
SegmentMap::iterator pub_it;
tf::Transform transform;
std::vector<tf::StampedTransform, std::allocator<tf::StampedTransform> > transforms;
geometry_msgs::TransformStampedPtr pose_msg(new geometry_msgs::TransformStamped);
static unsigned int cnt = 0;
for (unsigned int i_subjects = 0; i_subjects < n_subjects; i_subjects++)
{
subject_name = msvcbridge::GetSubjectName(i_subjects).SubjectName;
unsigned int n_segments = msvcbridge::GetSegmentCount(subject_name).SegmentCount;
for (unsigned int i_segments = 0; i_segments < n_segments; i_segments++)
{
segment_name = msvcbridge::GetSegmentName(subject_name, i_segments).SegmentName;
Output_GetSegmentGlobalTranslation trans = msvcbridge::GetSegmentGlobalTranslation(subject_name, segment_name);
Output_GetSegmentGlobalRotationQuaternion quat = msvcbridge::GetSegmentGlobalRotationQuaternion(subject_name,
segment_name);
if (trans.Result == Result::Success && quat.Result == Result::Success)
{
if (!trans.Occluded && !quat.Occluded)
{
transform.setOrigin(tf::Vector3(trans.Translation[0] / 1000, trans.Translation[1] / 1000,
trans.Translation[2] / 1000));
transform.setRotation(tf::Quaternion(quat.Rotation[0], quat.Rotation[1], quat.Rotation[2],
quat.Rotation[3]));
tracked_frame = tracked_frame_suffix_ + "/" + subject_name + "/" + segment_name;
boost::mutex::scoped_try_lock lock(segments_mutex_);
if (lock.owns_lock())
{
pub_it = segment_publishers_.find(subject_name + "/" + segment_name);
if (pub_it != segment_publishers_.end())
{
SegmentPublisher & seg = pub_it->second;
//ros::Time thisTime = now_time - ros::Duration(latencyInMs / 1000);
if (seg.is_ready)
{
transform = transform * seg.calibration_pose;
transforms.push_back(tf::StampedTransform(transform, frame_time, tf_ref_frame_id_, tracked_frame));
// transform = tf::StampedTransform(flyer_transform, frame_time, tf_ref_frame_id_, tracked_frame);
// tf_broadcaster_.sendTransform(transform);
tf::transformStampedTFToMsg(transforms.back(), *pose_msg);
seg.pub.publish(pose_msg);
}
}
else
{
lock.unlock();
createSegment(subject_name, segment_name);
}
}
}
else
{
if (cnt % 100 == 0)
ROS_WARN_STREAM("" << subject_name <<" occluded, not publishing... " );
}
}
else
{
ROS_WARN("GetSegmentGlobalTranslation/Rotation failed (result = %s, %s), not publishing...",
Adapt(trans.Result).c_str(), Adapt(quat.Result).c_str());
}
}
}
if (broadcast_tf_)
tf_broadcaster_->sendTransform(transforms);
cnt++;
}
