/**
* Read sto file.
*
* @param aFilename name of sto file.
*/
void MarkerData::readStoFile(const string& aFileName)
{
if (aFileName.empty())
throw Exception("MarkerData.readStoFile: ERROR- Marker file name is empty",__FILE__,__LINE__);
// If the file was written by STOFileAdapter, make the file readable by
// Storage. Calls below have no effect otherwise.
std::string tmpFileName{"tmp.sto"};
bool versionChanged{revertToVersionNumber1(aFileName, tmpFileName)};
if(versionChanged)
addNumRowsNumColumns(tmpFileName, aFileName);
std::remove(tmpFileName.c_str());
Storage store(aFileName);
// populate map between marker names and column numbers
std::map<int, std::string> markerIndices;
buildMarkerMap(store, markerIndices);
if (markerIndices.size()==0){
throw Exception("MarkerData.readStoFile: ERROR- No markers were identified. Markers should appear on consecutive columns as Marker1.x Marker1.y Marker1.z Marker2.x... etc.",__FILE__,__LINE__);
}
std::map<int, std::string>::iterator iter;
for (iter = markerIndices.begin(); iter != markerIndices.end(); iter++) {
SimTK::String markerNameWithSuffix = iter->second;
size_t dotIndex =
SimTK::String::toLower(markerNameWithSuffix).find_last_of(".x");
SimTK::String candidateMarkerName = markerNameWithSuffix.substr(0, dotIndex-1);
_markerNames.append(candidateMarkerName);
}
// use map to populate data for MarkerData header
_numMarkers = (int) markerIndices.size();
_numFrames = store.getSize();
_firstFrameNumber = 1;
_dataRate = 250;
_cameraRate = 250;
_originalDataRate = 250;
_originalStartFrame = 1;
_originalNumFrames = _numFrames;
_fileName = aFileName;
_units = Units(Units::Meters);
double time;
int sz = store.getSize();
for (int i=0; i < sz; i++){
StateVector* nextRow = store.getStateVector(i);
time = nextRow->getTime();
int frameNum = i+1;
MarkerFrame *frame = new MarkerFrame(_numMarkers, frameNum, time, _units);
const Array<double>& rowData = nextRow->getData();
// Cycle through map and add Marker coordinates to the frame. Same order as header.
for (iter = markerIndices.begin(); iter != markerIndices.end(); iter++) {
int startIndex = iter->first; // startIndex includes time but data doesn't!
frame->addMarker(SimTK::Vec3(rowData[startIndex-1], rowData[startIndex], rowData[startIndex+1]));
}
_frames.append(frame);
}
}
/**
* Average all the frames between aStartTime and
* aEndTime (inclusive) and store the result in the first
* frame. All other frames are deleted. The time and frame
* number of this one remaining frame are copied from the
* startIndex frame. The aThreshold parameter is for printing
* a warning if any marker moves more than that amount in
* the averaged frames. aThreshold is specified by the user,
* and is assumed to be in the units of the marker data.
*
* @param aThreshold amount of marker movement that is allowed for averaging.
* @param aStartTime start time of frame range to average.
* @param aEndTime end time of frame range to average.
*/
void MarkerData::averageFrames(double aThreshold, double aStartTime, double aEndTime)
{
if (_numFrames < 2)
return;
int startIndex = 0, endIndex = 1;
double *minX = NULL, *minY = NULL, *minZ = NULL, *maxX = NULL, *maxY = NULL, *maxZ = NULL;
findFrameRange(aStartTime, aEndTime, startIndex, endIndex);
MarkerFrame *averagedFrame = new MarkerFrame(*_frames[startIndex]);
/* If aThreshold is greater than zero, then calculate
* the movement of each marker so you can check if it
* is greater than aThreshold.
*/
if (aThreshold > 0.0)
{
minX = new double [_numMarkers];
minY = new double [_numMarkers];
minZ = new double [_numMarkers];
maxX = new double [_numMarkers];
maxY = new double [_numMarkers];
maxZ = new double [_numMarkers];
for (int i = 0; i < _numMarkers; i++)
{
minX[i] = minY[i] = minZ[i] = SimTK::Infinity;
maxX[i] = maxY[i] = maxZ[i] = -SimTK::Infinity;
}
}
/* Initialize all the averaged marker locations to 0,0,0. Then
* loop through the frames to be averaged, adding each marker location
* to averagedFrame. Keep track of the min/max XYZ for each marker
* so you can compare it to aThreshold when you're done.
*/
for (int i = 0; i < _numMarkers; i++)
{
int numFrames = 0;
Vec3& avePt = averagedFrame->updMarker(i);
avePt = Vec3(0);
for (int j = startIndex; j <= endIndex; j++)
{
Vec3& pt = _frames[j]->updMarker(i);
if (!pt.isNaN())
{
Vec3& coords = pt; //.get();
avePt += coords;
numFrames++;
if (aThreshold > 0.0)
{
if (coords[0] < minX[i])
minX[i] = coords[0];
if (coords[0] > maxX[i])
maxX[i] = coords[0];
if (coords[1] < minY[i])
minY[i] = coords[1];
if (coords[1] > maxY[i])
maxY[i] = coords[1];
if (coords[2] < minZ[i])
minZ[i] = coords[2];
if (coords[2] > maxZ[i])
maxZ[i] = coords[2];
}
}
}
/* Now divide by the number of frames to get the average. */
if (numFrames > 0)
avePt /= (double)numFrames;
else
avePt = Vec3(SimTK::NaN) ;//(SimTK::NaN, SimTK::NaN, SimTK::NaN);
}
/* Store the indices from the file of the first frame and
* last frame that were averaged, so you can report them later.
*/
int startUserIndex = _frames[startIndex]->getFrameNumber();
int endUserIndex = _frames[endIndex]->getFrameNumber();
/* Now delete all the existing frames and insert the averaged one. */
_frames.clearAndDestroy();
_frames.append(averagedFrame);
_numFrames = 1;
_firstFrameNumber = _frames[0]->getFrameNumber();
if (aThreshold > 0.0)
{
for (int i = 0; i < _numMarkers; i++)
{
Vec3& pt = _frames[0]->updMarker(i);
if (pt.isNaN())
{
cout << "___WARNING___: marker " << _markerNames[i] << " is missing in frames " << startUserIndex
<< " to " << endUserIndex << ". Coordinates will be set to NAN." << endl;
}
else if (maxX[i] - minX[i] > aThreshold ||
maxY[i] - minY[i] > aThreshold ||
maxZ[i] - minZ[i] > aThreshold)
{
double maxDim = maxX[i] - minX[i];
maxDim = MAX(maxDim, (maxY[i] - minY[i]));
maxDim = MAX(maxDim, (maxZ[i] - minZ[i]));
cout << "___WARNING___: movement of marker " << _markerNames[i] << " in " << _fileName
<< " is " << maxDim << " (threshold = " << aThreshold << ")" << endl;
}
}
}
cout << "Averaged frames from time " << aStartTime << " to " << aEndTime << " in " << _fileName
<< " (frames " << startUserIndex << " to " << endUserIndex << ")" << endl;
if (aThreshold > 0.0)
{
delete [] minX;
delete [] minY;
delete [] minZ;
delete [] maxX;
delete [] maxY;
delete [] maxZ;
}
}
/**
* Read TRC file.
*
* @param aFilename name of TRC file.
* @param aSMD MarkerData object to hold the file contents
*/
void MarkerData::readTRCFile(const string& aFileName, MarkerData& aSMD)
{
ifstream in;
string line, buffer;
int frameNum, coordsRead;
double time;
SimTK::Vec3 coords;
if (aFileName.empty())
throw Exception("MarkerData.readTRCFile: ERROR- Marker file name is empty",__FILE__,__LINE__);
in.open(aFileName.c_str());
if (!in.good())
{
string errorMessage;
errorMessage = "Unable to open marker file " + aFileName;
throw Exception(errorMessage);
}
readTRCFileHeader(in, aFileName, aSMD);
/* read frame data */
while (getline(in, line))
{
/* skip over any blank lines */
if (findFirstNonWhiteSpace(line) == -1)
continue;
if (aSMD._frames.getSize() == aSMD._numFrames)
{
#if 0
if (gUseGlobalMessages)
{
gErrorBuffer += "Extra data found at end of tracked marker file. ";
gErrorBuffer += "Header declared only " + intToString(trc->header.numFrames) + " frames.\n";
}
rc = smFileWarning;
#endif
break;
}
if (!readIntegerFromString(line, &frameNum))
{
#if 0
if (gUseGlobalMessages)
gErrorBuffer += "Could not read frame number in tracked marker file.\n";
rc = smFileError;
goto cleanup;
#endif
}
if (!readDoubleFromString(line, &time))
{
#if 0
if (gUseGlobalMessages)
gErrorBuffer += "Could not read time in tracked marker file.\n";
rc = smFileError;
goto cleanup;
#endif
}
MarkerFrame *frame = new MarkerFrame(aSMD._numMarkers, frameNum, time, aSMD._units);
/* keep reading sets of coordinates until the end of the line is
* reached. If more coordinates were read than there are markers,
* return an error.
*/
coordsRead = 0;
bool allowNaNs = true;
while (readCoordinatesFromString(line, &coords[0], allowNaNs))
{
if (coordsRead >= aSMD._numMarkers)
{
break;
#if 0 // Don't return an error because many TRC files have extra data at the ends of rows
if (gUseGlobalMessages)
gErrorBuffer += "Extra data found in frame number " + intToString(frameNum) +
" in tracked marker file.\n";
rc = smFileError;
// delete the current markerCoordList because framesRead has not been incremented yet.
delete [] f->markerCoordList;
goto cleanup;
#endif
}
if (coordsRead < aSMD._numMarkers)
frame->addMarker(coords);
coordsRead++;
}
if (coordsRead < aSMD._numMarkers)
{
#if 0
if (gUseGlobalMessages)
gErrorBuffer += " Missing data in frame number " + intToString(frameNum) +
" in tracked marker file.\n";
rc = smFileError;
// delete the current markerCoordList because framesRead has not been incremented yet.
delete [] f->markerCoordList;
goto cleanup;
#endif
}
aSMD._frames.append(frame);
}
if (aSMD._frames.getSize() < aSMD._numFrames)
{
#if 0
if (gUseGlobalMessages)
gErrorBuffer += "Missing data in tracked marker file. Only " + intToString(framesRead) + " of " +
intToString(trc->header.numFrames) + " frames found.\n";
rc = smFileError;
goto cleanup;
#endif
aSMD._numFrames = aSMD._frames.getSize();
}
/* If the user-defined frame numbers are not continguous from the first frame to the
* last, reset them to a contiguous array. This is necessary because the user-defined
* numbers are used to index the array of frames.
*/
if (aSMD._frames[aSMD._numFrames-1]->getFrameNumber() - aSMD._frames[0]->getFrameNumber() !=
aSMD._numFrames - 1)
{
int firstIndex = aSMD._frames[0]->getFrameNumber();
for (int i = 1; i < aSMD._numFrames; i++)
aSMD._frames[i]->setFrameNumber(firstIndex + i);
}
#if 0
if (gUseGlobalMessages)
{
gMessage += "TRC file " + actualFileName + "\n\t" + intToString(trc->header.numFrames)
+ " frames\n\t" + intToString(trc->header.numMarkers) + " markers/frame\n";
gMessage += "Read " + intToString(framesRead) + " frames.\n";
}
#endif
//cleanup:
in.close();
}
