void ITMFileBasedTracker::TrackCamera(ITMTrackingState *trackingState, const ITMView *view)
{
trackingState->trackerResult = ITMTrackingState::TRACKING_FAILED;
// Try to open the file
std::ifstream poseFile(GetCurrentFilename().c_str());
// Always increment frameCount, this allows skipping missing files that could correspond
// to frames where tracking failed during capture.
++frameCount;
// File not found, signal tracking failure.
if (!poseFile)
{
return;
}
Matrix4f invPose;
// Matrix is column-major
poseFile >> invPose.m00 >> invPose.m10 >> invPose.m20 >> invPose.m30
>> invPose.m01 >> invPose.m11 >> invPose.m21 >> invPose.m31
>> invPose.m02 >> invPose.m12 >> invPose.m22 >> invPose.m32
>> invPose.m03 >> invPose.m13 >> invPose.m23 >> invPose.m33;
if (poseFile)
{
// No read errors, tracking is assumed good
trackingState->trackerResult = ITMTrackingState::TRACKING_GOOD;
trackingState->pose_d->SetInvM(invPose);
}
}
Map::Map(std::string directory) {
boost::filesystem::path base(directory);
locationOnDisk = directory;
std::ifstream commandFile((base / "speeds.sl").string().c_str());
loadCommands(commandFile);
std::ifstream poseFile((base / "positions.pl").string().c_str());
loadPositions(poseFile);
std::ifstream anchorPointsFile((base / "anchorPoints.apd").string().c_str());
loadAnchorPoints(anchorPointsFile);
}
StaticPoseNode::StaticPoseNode(ros::NodeHandle & n)
: n_(n)
, n_param_("~")
, frequency_(FREQUENCY)
, publishTF_(PUBLISH_TF)
, sync_with_pub_(SYNC_WITH_A_PUBLISHER) {
n_param_.getParam ( "frequency", frequency_ );
ROS_INFO("\tfrequency: %5.2f", frequency_);
std::string poseFile(POSE_FILE);
n_param_.getParam ( "pose_file", poseFile );
ROS_INFO("\tpose_file: %s", poseFile.c_str());
std::string poseFileFolder(POSE_FILE_FOLDER);
n_param_.getParam ( "pose_file_folder", poseFileFolder );
ROS_INFO("\tpose_file_folder: %s", poseFileFolder.c_str());
std::string poseFileFolderRegex(POSE_FILE_REGEX);
n_param_.getParam ( "pose_file_folder_regex", poseFileFolderRegex );
ROS_INFO("\tpose_file_folder_regex: %s", poseFileFolderRegex.c_str());
n_param_.getParam ( "publish_tf", publishTF_ );
ROS_INFO("\tpublish_tf: %s", (publishTF_?"true":"false") );
n_param_.getParam ( "sync", sync_with_pub_ );
ROS_INFO("\tsync: %s", (sync_with_pub_?"true":"false") );
if (sync_with_pub_) {
sub_ = n_.subscribe("camera_info", 1000, &StaticPoseNode::callbackSync, this);
}
readFile(poseFile);
readFileFolder(poseFileFolder, poseFileFolderRegex);
initService();
}
