7 #include "PlusConfigure.h" 10 #ifdef PLUS_RENDERING_ENABLED 14 #include "vtkObjectFactory.h" 15 #include "vtkIGSIOTrackedFrameList.h" 16 #include "igsioTrackedFrame.h" 17 #include "vtkPoints.h" 18 #include "vtksys/SystemTools.hxx" 20 #include "vtkDoubleArray.h" 21 #include "vtkVariantArray.h" 48 this->Superclass::PrintSelf(os, indent);
49 os << indent <<
"Update time: " <<
UpdateTime.GetMTime() << std::endl;
50 os << indent <<
"Spacing: " << this->
Spacing[0] <<
" " << this->
Spacing[1] << std::endl;
52 os << indent <<
"Calibration error: mean=" << this->
ErrorMean <<
" stdev=" << this->
ErrorStdev << std::endl;
56 os << indent <<
"TrackedFrameList:" << std::endl;
62 os << indent <<
"ReportTable:" << std::endl;
71 LOG_TRACE(
"vtkPlusCenterOfRotationCalibAlgo::SetInputs");
88 LOG_TRACE(
"vtkPlusCenterOfRotationCalibAlgo::GetCenterOfRotationPx");
101 LOG_TRACE(
"vtkPlusCenterOfRotationCalibAlgo::GetError");
114 LOG_TRACE(
"vtkPlusCenterOfRotationCalibAlgo::Update");
116 if (this->GetMTime() < this->
UpdateTime.GetMTime())
118 LOG_DEBUG(
"Center of rotation calibration result is up-to-date!");
125 LOG_ERROR(
"Failed to perform calibration - tracked frame list is invalid");
130 std::vector<vnl_vector<double> > aMatrix;
131 std::vector<double> bVector;
135 LOG_ERROR(
"Unable to construct linear equation for center of rotation calibration algorithm!");
139 if (aMatrix.size() == 0 || bVector.size() == 0)
141 LOG_WARNING(
"Center of rotation calculation failed, no data found!");
146 vnl_vector<double> centerOfRotationInPx(2, 0);
149 LOG_WARNING(
"Failed to run LSQRMinimize!");
153 if (centerOfRotationInPx.empty() || centerOfRotationInPx.size() < 2)
155 LOG_ERROR(
"Unable to calibrate center of rotation! Minimizer returned empty result.");
174 int numberOfNWirePatterns = 0;
178 LOG_WARNING(
"Unable to compute number of N wire patterns - tracked frame list is NULL!");
179 return numberOfNWirePatterns;
182 for (
unsigned int frame = 0; frame < this->
TrackedFrameList->GetNumberOfTrackedFrames(); ++frame)
184 igsioTrackedFrame* trackedFrame = this->
TrackedFrameList->GetTrackedFrame(frame);
185 if (trackedFrame == NULL)
187 LOG_ERROR(
"Unable to get tracked frame from the list - tracked frame is NULL (position in the list: " << frame <<
")!");
191 vtkPoints* fiduacialPointsCoordinatePx = trackedFrame->GetFiducialPointsCoordinatePx();
192 if (fiduacialPointsCoordinatePx == NULL)
194 LOG_ERROR(
"Unable to get segmented fiducial points from tracked frame - FiducialPointsCoordinatePx is NULL, frame is not yet segmented (position in the list: " << frame <<
")!");
198 if (fiduacialPointsCoordinatePx->GetNumberOfPoints() == 0)
205 if (fiduacialPointsCoordinatePx->GetNumberOfPoints() % 3 != 0)
207 LOG_WARNING(
"Unrecognized wire patterns for frame #" << frame);
212 int patterns = fiduacialPointsCoordinatePx->GetNumberOfPoints() / 3;
215 if (numberOfNWirePatterns > 0 && patterns != numberOfNWirePatterns)
217 LOG_ERROR(
"Number of N-wire patterns different between frames!");
221 numberOfNWirePatterns = patterns;
224 return numberOfNWirePatterns;
230 LOG_TRACE(
"vtkPlusCenterOfRotationCalibAlgo::ConstructLinearEquationForCalibration");
236 LOG_ERROR(
"Failed to construct linear equation for center of rotation calibration - tracked frame list is NULL!");
243 int numberOfFrames = 0;
250 igsioTrackedFrame* trackedFrame = this->
TrackedFrameList->GetTrackedFrame(frameNumber);
253 if (trackedFrame->GetFiducialPointsCoordinatePx() == NULL)
259 if (trackedFrame->GetFiducialPointsCoordinatePx()->GetNumberOfPoints() == 0)
267 if (numberOfFrames < 30)
269 LOG_WARNING(
"Center of rotation calculation failed - there is not enough data (" << numberOfFrames <<
" out of at least 30)!");
274 std::vector<vtkSmartPointer<vtkPoints> > vectorOfWirePoints;
275 vectorOfWirePoints.reserve(numberOfFrames);
283 igsioTrackedFrame* trackedFrame = this->
TrackedFrameList->GetTrackedFrame(frameNumber);
285 if (trackedFrame->GetFiducialPointsCoordinatePx() == NULL)
287 LOG_ERROR(
"Unable to get segmented fiducial points from tracked frame - FiducialPointsCoordinatePx is NULL, frame is not yet segmented (position in the list: " << frameNumber <<
")!");
291 if (trackedFrame->GetFiducialPointsCoordinatePx()->GetNumberOfPoints() == 0)
296 vtkSmartPointer<vtkPoints> point = vtkSmartPointer<vtkPoints>::New();
297 point->SetDataTypeToDouble();
298 point->SetNumberOfPoints(numberOfNFiduacials * 2);
301 for (
int p = 0;
p < trackedFrame->GetFiducialPointsCoordinatePx()->GetNumberOfPoints(); ++
p)
303 if (((
p + 1) % 3) != 2)
305 double* wireCoordinatePx = trackedFrame->GetFiducialPointsCoordinatePx()->GetPoint(
p);
306 point->SetPoint(vectorID++, wireCoordinatePx[0], wireCoordinatePx[1], wireCoordinatePx[2]);
310 vectorOfWirePoints.push_back(point);
313 for (
unsigned int i = 0;
i <= vectorOfWirePoints.size() - 2;
i++)
315 for (
unsigned int j =
i + 1; j <= vectorOfWirePoints.size() - 1; j = j + 2)
317 if (vectorOfWirePoints[
i]->GetNumberOfPoints() != vectorOfWirePoints[j]->GetNumberOfPoints())
322 for (
int point = 0; point < vectorOfWirePoints[
i]->GetNumberOfPoints(); point++)
325 double Xi = vectorOfWirePoints[
i]->GetPoint(point)[0] * this->
Spacing[0];
326 double Yi = vectorOfWirePoints[
i]->GetPoint(point)[1] * this->
Spacing[1];
329 double Xj = vectorOfWirePoints[j]->GetPoint(point)[0] * this->
Spacing[0];
330 double Yj = vectorOfWirePoints[j]->GetPoint(point)[1] * this->
Spacing[1];
333 vnl_vector<double> rowOfDistance(2, 0);
334 rowOfDistance.put(0, Xi - Xj);
335 rowOfDistance.put(1, Yi - Yj);
336 aMatrix.push_back(rowOfDistance);
339 bVector.push_back(0.5 * (Xi * Xi + Yi * Yi - Xj * Xj - Yj * Yj));
350 LOG_TRACE(
"vtkPlusCenterOfRotationCalibAlgo::UpdateReportTable");
363 std::ostringstream columnNameRightRadius;
364 columnNameRightRadius <<
"Wire#" << (
i * 3 + 1) <<
"Radius";
367 std::ostringstream columnNameRightRadiusDistanceFromMean;
368 columnNameRightRadiusDistanceFromMean <<
"Wire#" << (
i * 3 + 1) <<
"RadiusDistanceFromMean";
371 std::ostringstream columnNameRightWireX;
372 columnNameRightWireX <<
"w" << (
i * 3 + 1) <<
"xPx";
375 std::ostringstream columnNameRightWireY;
376 columnNameRightWireY <<
"w" << (
i * 3 + 1) <<
"yPx";
379 std::ostringstream columnNameLeftRadius;
380 columnNameLeftRadius <<
"Wire#" << (
i * 3 + 3) <<
"Radius";
383 std::ostringstream columnNameLeftRadiusDistanceFromMean;
384 columnNameLeftRadiusDistanceFromMean <<
"Wire#" << (
i * 3 + 3) <<
"RadiusDistanceFromMean";
387 std::ostringstream columnNameLeftWireX;
388 columnNameLeftWireX <<
"w" << (
i * 3 + 3) <<
"xPx";
391 std::ostringstream columnNameLeftWireY;
392 columnNameLeftWireY <<
"w" << (
i * 3 + 3) <<
"yPx";
397 const double sX = this->
Spacing[0];
398 const double sY = this->
Spacing[1];
402 std::vector<double> probePosVector;
403 std::vector<double> probeRotVector;
404 std::vector<double> templatePosVector;
409 igsioTrackedFrame* frame = this->
TrackedFrameList->GetTrackedFrame(frameNumber);
411 if (frame->GetFiducialPointsCoordinatePx() == NULL
412 || frame->GetFiducialPointsCoordinatePx()->GetNumberOfPoints() == 0)
418 double probePos(0), probeRot(0), templatePos(0);
419 if (!igsioTrackedFrameEncoderPositionFinder::GetStepperEncoderValues(frame, probePos, probeRot, templatePos))
421 LOG_WARNING(
"Unable to get probe position from tracked frame info for frame #" << frameNumber);
424 probePosVector.push_back(probePos);
425 probeRotVector.push_back(probeRot);
426 templatePosVector.push_back(templatePos);
431 double w1x = frame->GetFiducialPointsCoordinatePx()->GetPoint(w * 3)[0];
432 double w1y = frame->GetFiducialPointsCoordinatePx()->GetPoint(w * 3)[1];
435 wirePositions[4 * w].push_back(w1x);
436 wirePositions[4 * w + 1].push_back(w1y);
437 wireRadiusVector[2 * w].push_back(w1Radius);
440 double w3x = frame->GetFiducialPointsCoordinatePx()->GetPoint(w * 3 + 2)[0];
441 double w3y = frame->GetFiducialPointsCoordinatePx()->GetPoint(w * 3 + 2)[1];
444 wirePositions[4 * w + 2].push_back(w3x);
445 wirePositions[4 * w + 3].push_back(w3y);
446 wireRadiusVector[2 * w + 1].push_back(w3Radius);
450 const int numberOfElements = wireRadiusVector[0].size();
453 for (
int i = 0;
i < numberOfElements; ++
i)
457 wireRadiusMean[w] += wireRadiusVector[w][
i] / (1.0 * numberOfElements);
462 for (
int i = 0;
i < numberOfElements; ++
i)
466 wireDistancesFromMeanRadius[w].push_back(wireRadiusMean[w] - wireRadiusVector[w][
i]);
470 for (
int row = 0; row < numberOfElements; ++row)
472 vtkSmartPointer<vtkVariantArray> tableRow = vtkSmartPointer<vtkVariantArray>::New();
474 tableRow->InsertNextValue(probePosVector[row]);
475 tableRow->InsertNextValue(probeRotVector[row]);
476 tableRow->InsertNextValue(templatePosVector[row]);
480 tableRow->InsertNextValue(wireRadiusVector[w][row]);
481 tableRow->InsertNextValue(wireDistancesFromMeanRadius[w][row]);
482 tableRow->InsertNextValue(wirePositions[2 * w][row]);
483 tableRow->InsertNextValue(wirePositions[2 * w + 1][row]);
486 if (tableRow->GetNumberOfTuples() == this->
ReportTable->GetNumberOfColumns())
492 LOG_WARNING(
"Unable to insert new row to center of rotation error table, number of columns are different (" 493 << tableRow->GetNumberOfTuples() <<
" vs. " << this->
ReportTable->GetNumberOfColumns() <<
").");
498 #ifdef PLUS_RENDERING_ENABLED 510 vtkSmartPointer<vtkTable> table = vtkSmartPointer<vtkTable>::New();
514 if (columnName == NULL)
516 LOG_ERROR(
"Failed to add new column to table - column name is NULL!");
520 if (this->
ReportTable->GetColumnByName(columnName) != NULL)
522 LOG_WARNING(
"Column name " << columnName <<
" already exist!");
527 vtkSmartPointer<vtkDoubleArray> col = vtkSmartPointer<vtkDoubleArray>::New();
528 col->SetName(columnName);
537 LOG_TRACE(
"vtkPlusCenterOfRotationCalibAlgo::GenerateReport");
542 LOG_ERROR(
"Unable to generate report - center of rotation axis calibration failed!");
554 LOG_TRACE(
"vtkPlusCenterOfRotationCalibAlgo::GenerateCenterOfRotationReport");
556 if (htmlReport == NULL)
558 LOG_ERROR(
"Caller should define HTML report generator before report generation!");
562 if (reportTable == NULL)
564 LOG_ERROR(
"Unable to generate report - input report table is NULL!");
569 std::string reportFileName =
vtkPlusConfig::GetInstance()->GetApplicationStartTimestamp() +
".CenterOfRotationCalculationError.txt";
571 #ifdef PLUS_RENDERING_ENABLED 574 LOG_ERROR(
"Failed to dump translation axis calibration report table to " << reportFile);
580 std::ostringstream report;
581 report <<
"Center of rotation (px): " << centerOfRotationPx[0] <<
" " << centerOfRotationPx[1] <<
"</br>" ;
585 const int numOfPlots = numberOfNWirePatterns * 2;
588 std::vector<int> wires(numOfPlots);
589 for (
int i = 0;
i < numberOfNWirePatterns; ++
i)
591 wires[2 *
i] =
i * 3 + 1;
592 wires[2 *
i + 1] =
i * 3 + 3;
595 for (
int i = 0;
i < numOfPlots;
i++)
597 std::ostringstream wireName;
598 wireName << wires[
i];
599 std::string wireFullName = std::string(
"Wire #") + wireName.str();
602 double valueRangeMin = -2.0;
603 double valueRangeMax = 2.0;
604 int numberOfBins = 41;
605 int imageSize[2] = {800, 400};
607 std::string outputImageFilename = std::string(
"w") + wireName.str() +
"-PositionError.png";
608 std::string outputImagePath = htmlReport->
AddImageAutoFilename(outputImageFilename.c_str(),
"Wire distance from transducer");
609 #ifdef PLUS_RENDERING_ENABLED 612 outputImageFilename = std::string(
"w") + wireName.str() +
"-AngleError.png";
613 outputImagePath = htmlReport->
AddImageAutoFilename(outputImageFilename.c_str(),
"Wire distance from transducer");
616 outputImageFilename = std::string(
"w") + wireName.str() +
"-ErrorHistogram.png";
617 outputImagePath = htmlReport->
AddImageAutoFilename(outputImageFilename.c_str(),
"Wire distance from transducer - error histogram");
618 PlusPlotter::WriteHistogramChartToFile(
"Wire distance from transducer - error histogram", *reportTable, 4 +
i * 4 , valueRangeMin, valueRangeMax, numberOfBins, imageSize, outputImagePath.c_str());
static PlusStatus WriteTableToFile(vtkTable &table, const std::string &filename)
virtual PlusStatus UpdateReportTable()
virtual void AddText(const char *text, HEADINGS h, const char *style=NULL)
std::string GetOutputPath(const std::string &subPath)
static PlusStatus LSQRMinimize(const std::vector< std::vector< double > > &aMatrix, const std::vector< double > &bVector, vnl_vector< double > &resultVector, double *mean=NULL, double *stdev=NULL, vnl_vector< unsigned int > *notOutliersIndices=NULL)
std::vector< int > TrackedFrameListIndices
vtkPlusCenterOfRotationCalibAlgo()
virtual PlusStatus GenerateReport(vtkPlusHTMLGenerator *htmlReport)
virtual ~vtkPlusCenterOfRotationCalibAlgo()
double CenterOfRotationPx[2]
Algorithm for computing the center of in-plane rotation of ultrasound images of a calibration phantom...
virtual void SetTrackedFrameList(vtkIGSIOTrackedFrameList *)
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
static vtkPlusConfig * GetInstance()
virtual void SetCenterOfRotationPx(int, int)
vtkIGSIOTrackedFrameList * TrackedFrameList
static PlusStatus GenerateCenterOfRotationReport(int numberOfNWirePatterns, vtkPlusHTMLGenerator *htmlReport, vtkTable *reportTable, double centerOfRotationPx[2])
virtual void AddHorizontalLine()
virtual void SetTrackedFrameListIndices(std::vector< int > &indices)
virtual void SetReportTable(vtkTable *)
vtkStandardNewMacro(vtkPlusCenterOfRotationCalibAlgo)
virtual PlusStatus GetCenterOfRotationPx(double centerOfRotationPx[2])
virtual void AddParagraph(const char *paragraph)
static PlusStatus WriteHistogramChartToFile(const std::string &chartTitle, vtkTable &inputTable, int inputColumnIndex, double valueRangeMin, double valueRangeMax, int numberOfBins, int imageSize[2], const std::string &outputImageFilename)
virtual std::string AddImageAutoFilename(const char *filenamePostfix, const char *description, const int widthPx=0, const int heightPx=0)
virtual PlusStatus ConstructLinearEquationForCalibration(std::vector< vnl_vector< double > > &aMatrix, std::vector< double > &bVector)
int GetNumberOfNWirePatterns()
virtual void SetSpacing(double, double)
class for generating basic html tags
PlusStatus AddNewColumnToReportTable(const char *columnName)
virtual PlusStatus Update()
static PlusStatus WriteScatterChartToFile(const std::string &chartTitle, const std::string &yAxisText, vtkTable &inputTable, int xColumnIndex, int yColumnIndex, int imageSize[2], const std::string &outputImageFilename)
virtual void SetInputs(vtkIGSIOTrackedFrameList *trackedFrameList, std::vector< int > &indices, double spacing[2])
virtual PlusStatus GetError(double &mean, double &stdev)