void ofxFBXScene::parseScene(FbxNode* pNode, FbxAnimLayer * pAnimLayer, ofxFBXNode * parentNode){
ofLogVerbose("ofxFBXScene") << "found node " << pNode->GetName() << " of type " << pNode->GetTypeName() << endl;
if( pNode->GetCamera() ){
ofxFBXNode * node = parseCameraInfo(pNode,pAnimLayer);
if(parentNode) node->getNode().setParent(parentNode->getNode());
parentNode = node;
}else if(pNode->GetMesh()){
ofxFBXNode * node = parseMeshInfo(pNode, pAnimLayer);
if(parentNode) node->getNode().setParent(parentNode->getNode());
parentNode = node;
}else if(pNode->GetLight()){
ofxFBXNode * node = parseLightInfo(pNode, pAnimLayer);
if(parentNode) node->getNode().setParent(parentNode->getNode());
parentNode = node;
}else{
ofxFBXNode node;
node.nodeName = pNode->GetName();
parsePositionCurve(node,pAnimLayer,pNode->LclTranslation);
parseScaleCurve(node,pAnimLayer,pNode->LclScaling);
parseRotationCurve(node,pAnimLayer,pNode,pNode->LclRotation);
nullsList.push_back(node);
if(parentNode) nullsList.back().getNode().setParent(parentNode->getNode());
parentNode = &nullsList.back();
}
//recursively traverse each node in the scene
int i, lCount = pNode->GetChildCount();
for (i = 0; i < lCount; i++)
{
parseScene(pNode->GetChild(i), pAnimLayer, parentNode);
}
}
void SuperFrameParser::fillNarrowMsg (const std::string ¶ms_file)
{
narrow_msgs_->header.frame_id = "/" + name_space_ + "/" + narrow_name_;
// big image has apperently some offset, adding the offset here
// big_img_msgs_->header.stamp.fromSec (timestamp_map_.find (file_name_)->second +
// (convertTicksToSeconds (super_frame_->header.frame.sf_version, super_frame_->header.frame.big.timestamp) -
// convertTicksToSeconds (super_frame_->header.frame.sf_version, super_frame_->header.frame.small.timestamp)));
std::map<std::string, double>::iterator it;
if ((it = timestamp_map_.find (file_name_)) == timestamp_map_.end ())
{
std::stringstream ss;
ss << "SuperFrame " << file_name_ << " not found in " << timestamp_name_;
throw std::runtime_error (ss.str ());
}
narrow_msgs_->header.stamp.fromSec (it->second);
narrow_msgs_->height = BIG_RGB_HEIGHT;
narrow_msgs_->width = BIG_RGB_WIDTH;
narrow_msgs_->step = narrow_msgs_->width/* * 2*/;
narrow_msgs_->encoding = sensor_msgs::image_encodings::MONO8;
narrow_msgs_->data.resize (narrow_msgs_->width * narrow_msgs_->height/* * 2*/);
memcpy (&narrow_msgs_->data[0], super_frame_->big_rgb, narrow_msgs_->data.size ());
///// fill in the camera infos ////////
std::vector<std::string> params;
parseCameraInfo (params_file, params);
narrow_info_->header.frame_id = narrow_msgs_->header.frame_id;
narrow_info_->header.stamp = narrow_msgs_->header.stamp;
narrow_info_->width = atoi (params[0].c_str ());
narrow_info_->height = atoi (params[1].c_str ());
narrow_info_->distortion_model = "devernay";
narrow_info_->D.resize (2);
narrow_info_->D[0] = atof (params[6].c_str ()); // omega
narrow_info_->D[1] = atof (params[7].c_str ()); // max_angle
/* Intrinsic camera matrix for the raw (distorted) images.
# [fx 0 cx]
# K = [ 0 fy cy]
# [ 0 0 1] */
boost::array<double, 9> K = { atof (params[2].c_str ()), 0.0, atof (params[4].c_str ()),
0.0, atof (params[3].c_str ()), atof (params[5].c_str ()),
0.0, 0.0, 1.0 } ;
narrow_info_->K = K;
/* # Projection/camera matrix
# [fx' 0 cx' Tx]
# P = [ 0 fy' cy' Ty]
# [ 0 0 1 0] */
boost::array<double, 12> P = { atof (params[2].c_str ()), 0.0, atof (params[4].c_str ()), 0.,
0.0, atof (params[3].c_str ()), atof (params[5].c_str ()), 0.,
0.0, 0.0, 1.0, 0. } ;
narrow_info_->P = P;
}
void SuperFrameParser::fillDepthMsg (const std::string ¶ms_file)
{
depth_msgs_->header.frame_id = "/" + name_space_ + "/" + depth_name_;
std::map<std::string, double>::iterator it;
if ((it = timestamp_map_.find (file_name_)) == timestamp_map_.end ())
{
std::stringstream ss;
ss << "SuperFrame " << file_name_ << " not found in " << timestamp_name_;
throw std::runtime_error (ss.str ());
}
// pre defined offset from depth to small image timestamp
// point_cloud_msgs_->header.stamp.fromSec (timestamp_map_.find (file_name_)->second + DEPTH_TIMESTAMP_OFFSET);
// calculate depth offset by getting the difference from the superframe timestamps
depth_msgs_->header.stamp.fromSec (it->second +
(convertTicksToSeconds (super_frame_->header.frame.sf_version, super_frame_->header.frame.depth.timestamp) -
convertTicksToSeconds (super_frame_->header.frame.sf_version, super_frame_->header.frame.small.timestamp)));
depth_msgs_->height = DEPTH_IMG_HEIGHT;
depth_msgs_->width = DEPTH_IMG_WIDTH;
depth_msgs_->step = depth_msgs_->width * 2;
depth_msgs_->encoding = sensor_msgs::image_encodings::MONO16;
depth_msgs_->data.resize (depth_msgs_->width * depth_msgs_->height * 2);
memcpy (&depth_msgs_->data[0], super_frame_->depth_img, depth_msgs_->data.size ());
///// fill in the camera infos ////////
std::vector<std::string> params;
parseCameraInfo (params_file, params);
depth_info_->header.frame_id = depth_msgs_->header.frame_id;
depth_info_->header.stamp = depth_msgs_->header.stamp;
depth_info_->width = atoi (params[0].c_str ());
depth_info_->height = atoi (params[1].c_str ());
depth_info_->distortion_model = "devernay";
depth_info_->D.resize (2);
depth_info_->D[0] = atof (params[6].c_str ()); // omega
depth_info_->D[1] = atof (params[7].c_str ()); // max_angle
/* Intrinsic camera matrix for the raw (distorted) images.
# [fx 0 cx]
# K = [ 0 fy cy]
# [ 0 0 1] */
boost::array<double, 9> K = { atof (params[2].c_str ()), 0.0, atof (params[4].c_str ()),
0.0, atof (params[3].c_str ()), atof (params[5].c_str ()),
0.0, 0.0, 1.0 } ;
depth_info_->K = K;
/* # Projection/camera matrix
# [fx' 0 cx' Tx]
# P = [ 0 fy' cy' Ty]
# [ 0 0 1 0] */
boost::array<double, 12> P = { atof (params[2].c_str ()), 0.0, atof (params[4].c_str ()), 0.,
0.0, atof (params[3].c_str ()), atof (params[5].c_str ()), 0.,
0.0, 0.0, 1.0, 0. } ;
depth_info_->P = P;
}
void SuperFrameParser::fillFisheyeMsg (const std::string ¶ms_file)
{
fisheye_msgs_->header.frame_id = "/" + name_space_ + "/" + fisheye_name_;
std::map<std::string, double>::iterator it;
if ((it = timestamp_map_.find (file_name_)) == timestamp_map_.end ())
{
std::stringstream ss;
ss << "SuperFrame " << file_name_ << " not found in " << timestamp_name_;
throw std::runtime_error (ss.str ());
}
fisheye_msgs_->header.stamp.fromSec (it->second);
fisheye_msgs_->height = SMALL_IMG_HEIGHT;
fisheye_msgs_->width = SMALL_IMG_WIDTH;
fisheye_msgs_->step = fisheye_msgs_->width;
fisheye_msgs_->encoding = sensor_msgs::image_encodings::MONO8;
fisheye_msgs_->data.resize (fisheye_msgs_->width * fisheye_msgs_->height);
memcpy (&fisheye_msgs_->data[0], super_frame_->small_img, fisheye_msgs_->data.size ());
///// fill in the camera infos ////////
std::vector<std::string> params;
parseCameraInfo (params_file, params);
fisheye_info_->header.frame_id = fisheye_msgs_->header.frame_id;
fisheye_info_->header.stamp = fisheye_msgs_->header.stamp;
fisheye_info_->width = atoi (params[0].c_str ());
fisheye_info_->height = atoi (params[1].c_str ());
fisheye_info_->distortion_model = "devernay";
fisheye_info_->D.resize (2);
fisheye_info_->D[0] = atof (params[6].c_str ()); // omega
fisheye_info_->D[1] = atof (params[7].c_str ()); // max_angle
/* Intrinsic camera matrix for the raw (distorted) images.
# [fx 0 cx]
# K = [ 0 fy cy]
# [ 0 0 1] */
boost::array<double, 9> K = { atof (params[2].c_str ()), 0.0, atof (params[4].c_str ()),
0.0, atof (params[3].c_str ()), atof (params[5].c_str ()),
0.0, 0.0, 1.0 } ;
fisheye_info_->K = K;
/* # Projection/camera matrix
# [fx' 0 cx' Tx]
# P = [ 0 fy' cy' Ty]
# [ 0 0 1 0] */
boost::array<double, 12> P = { atof (params[2].c_str ()), 0.0, atof (params[4].c_str ()), 0.,
0.0, atof (params[3].c_str ()), atof (params[5].c_str ()), 0.,
0.0, 0.0, 1.0, 0. } ;
fisheye_info_->P = P;
}
