void testOnePivotCalibration(csvCIS_pointCloudData::TrackerDevices trackerIndexedData, Eigen::Vector3d checkOutput, std::string description = "", bool debug = false) {
Eigen::VectorXd result = pivotCalibration(trackerIndexedData,debug);
Eigen::Vector3d checkResultFirst = result.block<3,1>(0,0);
Eigen::Vector3d checkResultSecond = result.block<3,1>(3,0);
// note: isApprox is known to break near zero
BOOST_CHECK(isWithinTolerance(checkResultSecond,checkOutput));
if (!isWithinTolerance(checkResultSecond,checkOutput)) {
std::cout << "\n\n" << description << "\n\n";
std::cout << "===============================================\n\n";
std::cout << "\n\nresult:\n\n" << result << "\n\n";
std::cout << "\n\ncheckresult FULL:\n\n" << result << "\n\n";
std::cout << "\n\ncheckresult FIRST:\n\n" << checkResultFirst << "\n\ncheckresult SECOND:\n\n" << checkResultSecond << "\n\ncheckoutput:\n\n" << checkOutput << "\n\n";
}
};
void testTwoPivotCalibration(std::string relativeDataPath,std::string datapathsuffix, bool debug = false){
AlgorithmData ad;
csvCIS_pointCloudData::TrackerFrames trackerIndexedData;
ad = initAlgorithmData(relativeDataPath,datapathsuffix);
trackerIndexedData = swapIndexing(ad.optpivot.frames);
/// @todo contains two different ways of attempting to do this worked on during debugging
/// @todo The core functions are likely correct, but we believe that we are using one of the wrong frames, the wrong calibration object, or the wrong ordering and that is causing the offset. We expect a data sourcing and frame transform ordering error rather than a flaw in the underlying algorithms.
Eigen::VectorXd optProbePivotPtAndCalInOptCoord = pivotCalibration(trackerIndexedData[1],debug);
if(debug) std::cout << "\n\ntestTwoPivot - p:\n\n" << optProbePivotPtAndCalInOptCoord << "\n\n";
Eigen::Vector3d optProbeCalInOptCoord = optProbePivotPtAndCalInOptCoord.block<3,1>(0,0);
Eigen::Vector3d optProbePivotPtInOptCoord = optProbePivotPtAndCalInOptCoord.block<3,1>(3,0);
Eigen::VectorXd emTrackerPivotPtAndCalInOptCoord = pivotCalibration(trackerIndexedData[0],debug);
Eigen::Vector3d emCalInOptCoord = emTrackerPivotPtAndCalInOptCoord.block<3,1>(0,0);
Eigen::Vector3d emTrackerPivotPtInOptCoord = emTrackerPivotPtAndCalInOptCoord.block<3,1>(3,0);
Eigen::VectorXd result = pivotCalibrationTwoSystems(trackerIndexedData[0],trackerIndexedData[1],debug);
Eigen::MatrixXd dcloud = registrationToFirstCloud(trackerIndexedData[0]);
Eigen::MatrixXd hcloud = registrationToFirstCloud(trackerIndexedData[1]);
//
// Eigen::Affine3d FDinv;
// FDinv.matrix() = homogeneousInverse(FD);
// FDinv.matrix() = Eigen::Matrix4d(FD.block<4,4>(0,0));
//
// Eigen::Vector3d optPivotPtInEMCoord = FDinv*optProbePivotPtInOptCoord;
// skip tranform into other coords for now
Eigen::Vector3d optPivotPtInEMCoord = optProbePivotPtInOptCoord;
Eigen::Vector3d groundTruthOptPivotPtInEMCoord = ad.output1.estOpticalPostPos;
bool isWithinTolerance_ = isWithinTolerance(optPivotPtInEMCoord,groundTruthOptPivotPtInEMCoord);
if (debug || !isWithinTolerance_) {
std::cout << "testing " <<relativeDataPath<<datapathsuffix<<"\n";
std::cout << "===============================================\n\n";
}
BOOST_CHECK(isWithinTolerance(optPivotPtInEMCoord,groundTruthOptPivotPtInEMCoord));
if(debug || !isWithinTolerance_){
Print(trackerIndexedData[0],true,"emTrackerIndexedData0"); // em tracker
Print(trackerIndexedData[1],true,"opticalProbeIndexedData1"); // optical probe
}
if (!isWithinTolerance_) {
std::cout << "\n\noptProbePivotPtInOptCoord:\n\n" << optProbePivotPtInOptCoord << "\n\n";
std::cout << "\n\ngroundTruthOptPivotPtInEMCoord:\n\n" << groundTruthOptPivotPtInEMCoord << "\n\n";
std::cout << "\n\nemTrackerPivotPtInOptCoord:\n\n" << emTrackerPivotPtInOptCoord << "\n\n";
//std::cout << "\n\FD:\n\n" << FD << "\n\n";
//std::cout << "\n\FH:\n\n" << FH << "\n\n";
}
};
void FillPlane::createEdgesOnPlane(vtkUnstructuredGrid *edge_grid)
{
vtkIdType num_edges = 0;
vtkIdType num_nodes = 0;
QVector<bool> is_edge_node(m_Grid->GetNumberOfPoints(), false);
for (vtkIdType id_face = 0; id_face < m_Grid->GetNumberOfCells(); ++id_face) {
vtkIdType num_pts, *pts;
if (isSurface(id_face, m_Grid)) {
m_Grid->GetCellPoints(id_face, num_pts, pts);
for (int i = 0; i < num_pts; ++i) {
if (m_Part.c2cGG(id_face, i) == -1) {
vtkIdType id_node1 = pts[i];
vtkIdType id_node2 = pts[0];
if (i < num_pts - 1) {
id_node2 = pts[i + 1];
}
vec3_t x1, x2;
m_Grid->GetPoint(id_node1, x1.data());
m_Grid->GetPoint(id_node2, x2.data());
if (isWithinTolerance(x1) && isWithinTolerance(x2)) {
is_edge_node[id_node1] = true;
is_edge_node[id_node2] = true;
++num_edges;
}
}
}
}
}
QVector<vtkIdType> node_map(m_Grid->GetNumberOfPoints(), -1);
for (vtkIdType id_node1 = 0; id_node1 < m_Grid->GetNumberOfPoints(); ++id_node1) {
if (is_edge_node[id_node1]) {
node_map[id_node1] = num_nodes;
++num_nodes;
}
}
m_NodeMap.resize(num_nodes);
allocateGrid(edge_grid, 2*num_edges, num_nodes, false);
for (vtkIdType id_node1 = 0; id_node1 < m_Grid->GetNumberOfPoints(); ++id_node1) {
if (node_map[id_node1] != -1) {
m_NodeMap[node_map[id_node1]] = id_node1;
vec3_t x;
m_Grid->GetPoint(id_node1, x.data());
edge_grid->GetPoints()->SetPoint(node_map[id_node1], x.data());
}
}
for (vtkIdType id_face = 0; id_face < m_Grid->GetNumberOfCells(); ++id_face) {
vtkIdType num_pts, *pts;
if (isSurface(id_face, m_Grid)) {
m_Grid->GetCellPoints(id_face, num_pts, pts);
for (int i = 0; i < num_pts; ++i) {
if (m_Part.c2cGG(id_face, i) == -1) {
vtkIdType id_node1 = pts[i];
vtkIdType id_node2 = pts[0];
if (i < num_pts - 1) {
id_node2 = pts[i + 1];
}
if (is_edge_node[id_node1] && is_edge_node[id_node2]) {
vtkIdType pts[2];
pts[0] = node_map[id_node1];
pts[1] = node_map[id_node2];
edge_grid->InsertNextCell(VTK_LINE, 2, pts);
}
}
}
}
}
}
SbRotation
SbSphereSectionProjector::getRotation(const SbVec3f &p1, const SbVec3f &p2)
//
////////////////////////////////////////////////////////////////////////
{
SbBool tol1 = isWithinTolerance(p1);
SbBool tol2 = isWithinTolerance(p2);
if (tol1 && tol2) {
// both points in tolerance, rotate about
// sphere center
return SbRotation(
p1 - sphere.getCenter(),
p2 - sphere.getCenter());
}
else if (!tol1 && !tol2) {
// both points out of tolerance, rotate about
// plane point
// Would like to just use this:
SbRotation badRot = SbRotation(p1 - planePoint, p2 - planePoint);
// but fp instablity gives back a goofy axis, so we don't get
// pure roll.
// So we need to snap the axis to be parallel to plane dir
SbVec3f badAxis; float goodAngle;
badRot.getValue(badAxis, goodAngle);
SbVec3f goodAxis;
if (badAxis.dot(planeDir) > 0.0)
goodAxis = planeDir;
else
goodAxis = -planeDir;
SbRotation rollRot(goodAxis, goodAngle);
//Now find rotation in the direction perpendicular to this:
SbVec3f diff1 = p1 - planePoint;
SbVec3f diff2 = p2 - planePoint;
float d = diff2.length() - diff1.length();
// Check for degenerate cases
float theta = d / sphere.getRadius();
if ( fabs(theta) < 0.000001 || fabs(theta) > 1.0 )
return rollRot;
diff1.normalize();
SbVec3f pullAxis = planeDir.cross( diff1 );
pullAxis.normalize();
SbRotation pullRot(pullAxis, getRadialFactor() * theta );
SbRotation totalRot = rollRot * pullRot;
return totalRot;
}
else {
// one point in, one point out, so rotate about
// the center of the sphere from the point on the
// sphere to the intersection of the plane and the
// sphere closest to the point off the sphere
SbLine planeLine;
SbVec3f intersection;
if (tol1) {
planeLine.setValue(planePoint, p2);
}
else {
planeLine.setValue(planePoint, p1);
}
if (! sphere.intersect(planeLine, intersection))
#ifdef DEBUG
SoDebugError::post("SbSphereSectionProjector::getRotation",
"Couldn't intersect plane line with sphere");
#else
/* Do nothing */;
#endif
if (tol1) {
// went off sphere
return SbRotation(
p1 - sphere.getCenter(),
intersection - sphere.getCenter());
}
else {
// came on to sphere
// "Hey cutie. You've got quite a radius..."
return SbRotation(
intersection - sphere.getCenter(),
p2 - sphere.getCenter());
}
}
}
