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(); }