Curve *EllipticalArc::transformed(Affine const& m) const
{
Ellipse e(center(X), center(Y), ray(X), ray(Y), _rot_angle);
Ellipse et = e.transformed(m);
Point inner_point = pointAt(0.5);
return et.arc( initialPoint() * m,
inner_point * m,
finalPoint() * m,
isSVGCompliant() );
}
void Path::do_append(Curve *c) {
boost::shared_ptr<Curve> curve(c);
if ( get_curves().front().get() == final_ ) {
final_->setPoint(1, curve->initialPoint());
} else {
if (curve->initialPoint() != finalPoint()) {
THROW_CONTINUITYERROR();
}
}
get_curves().insert(get_curves().end()-1, curve);
final_->setPoint(0, curve->finalPoint());
}
Path &Path::operator*=(Affine const &m) {
unshare();
Sequence::iterator last = get_curves().end() - 1;
Sequence::iterator it;
Point prev;
for (it = get_curves().begin() ; it != last ; ++it) {
*it = boost::shared_ptr<Curve>((*it)->transformed(m));
if ( it != get_curves().begin() && (*it)->initialPoint() != prev ) {
THROW_CONTINUITYERROR();
}
prev = (*it)->finalPoint();
}
for ( int i = 0 ; i < 2 ; ++i ) {
final_->setPoint(i, (*final_)[i] * m);
}
if (get_curves().size() > 1) {
if ( front().initialPoint() != initialPoint() || back().finalPoint() != finalPoint() ) {
THROW_CONTINUITYERROR();
}
}
return *this;
}
D2<SBasis> EllipticalArc::toSBasis() const
{
D2<SBasis> arc;
// the interval of parametrization has to be [0,1]
Coord et = initialAngle().radians() + ( _sweep ? sweepAngle() : -sweepAngle() );
Linear param(initialAngle(), et);
Coord cos_rot_angle, sin_rot_angle;
sincos(_rot_angle, sin_rot_angle, cos_rot_angle);
// order = 4 seems to be enough to get a perfect looking elliptical arc
SBasis arc_x = ray(X) * cos(param,4);
SBasis arc_y = ray(Y) * sin(param,4);
arc[0] = arc_x * cos_rot_angle - arc_y * sin_rot_angle + Linear(center(X),center(X));
arc[1] = arc_x * sin_rot_angle + arc_y * cos_rot_angle + Linear(center(Y),center(Y));
// ensure that endpoints remain exact
for ( int d = 0 ; d < 2 ; d++ ) {
arc[d][0][0] = initialPoint()[d];
arc[d][0][1] = finalPoint()[d];
}
return arc;
}
/*
* NOTE: this implementation follows Standard SVG 1.1 implementation guidelines
* for elliptical arc curves. See Appendix F.6.
*/
void EllipticalArc::_updateCenterAndAngles(bool svg)
{
Point d = initialPoint() - finalPoint();
// TODO move this to SVGElipticalArc?
if (svg)
{
if ( initialPoint() == finalPoint() )
{
_rot_angle = _start_angle = _end_angle = 0;
_center = initialPoint();
_rays = Geom::Point(0,0);
_large_arc = _sweep = false;
return;
}
_rays[X] = std::fabs(_rays[X]);
_rays[Y] = std::fabs(_rays[Y]);
if ( are_near(ray(X), 0) || are_near(ray(Y), 0) )
{
_rays[X] = L2(d) / 2;
_rays[Y] = 0;
_rot_angle = std::atan2(d[Y], d[X]);
_start_angle = 0;
_end_angle = M_PI;
_center = middle_point(initialPoint(), finalPoint());
_large_arc = false;
_sweep = false;
return;
}
}
else
{
if ( are_near(initialPoint(), finalPoint()) )
{
if ( are_near(ray(X), 0) && are_near(ray(Y), 0) )
{
_start_angle = _end_angle = 0;
_center = initialPoint();
return;
}
else
{
THROW_RANGEERROR("initial and final point are the same");
}
}
if ( are_near(ray(X), 0) && are_near(ray(Y), 0) )
{ // but initialPoint != finalPoint
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(X) == 0 && ray(Y) == 0 but initialPoint != finalPoint"
);
}
if ( are_near(ray(Y), 0) )
{
Point v = initialPoint() - finalPoint();
if ( are_near(L2sq(v), 4*ray(X)*ray(X)) )
{
Angle angle(v);
if ( are_near( angle, _rot_angle ) )
{
_start_angle = 0;
_end_angle = M_PI;
_center = v/2 + finalPoint();
return;
}
angle -= M_PI;
if ( are_near( angle, _rot_angle ) )
{
_start_angle = M_PI;
_end_angle = 0;
_center = v/2 + finalPoint();
return;
}
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(Y) == 0 "
"and slope(initialPoint - finalPoint) != rotation_angle "
"and != rotation_angle + PI"
);
}
if ( L2sq(v) > 4*ray(X)*ray(X) )
{
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(Y) == 0 and distance(initialPoint, finalPoint) > 2*ray(X)"
);
}
else
{
THROW_RANGEERROR(
"there is infinite ellipses that satisfy the given constraints: "
"ray(Y) == 0 and distance(initialPoint, finalPoint) < 2*ray(X)"
);
}
}
if ( are_near(ray(X), 0) )
{
Point v = initialPoint() - finalPoint();
if ( are_near(L2sq(v), 4*ray(Y)*ray(Y)) )
{
double angle = std::atan2(v[Y], v[X]);
if (angle < 0) angle += 2*M_PI;
double rot_angle = _rot_angle.radians() + M_PI/2;
if ( !(rot_angle < 2*M_PI) ) rot_angle -= 2*M_PI;
if ( are_near( angle, rot_angle ) )
{
_start_angle = M_PI/2;
_end_angle = 3*M_PI/2;
_center = v/2 + finalPoint();
return;
}
angle -= M_PI;
if ( angle < 0 ) angle += 2*M_PI;
if ( are_near( angle, rot_angle ) )
{
_start_angle = 3*M_PI/2;
_end_angle = M_PI/2;
_center = v/2 + finalPoint();
return;
}
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(X) == 0 "
"and slope(initialPoint - finalPoint) != rotation_angle + PI/2 "
"and != rotation_angle + (3/2)*PI"
);
}
if ( L2sq(v) > 4*ray(Y)*ray(Y) )
{
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints: "
"ray(X) == 0 and distance(initialPoint, finalPoint) > 2*ray(Y)"
);
}
else
{
THROW_RANGEERROR(
"there is infinite ellipses that satisfy the given constraints: "
"ray(X) == 0 and distance(initialPoint, finalPoint) < 2*ray(Y)"
);
}
}
}
Rotate rm(_rot_angle);
Affine m(rm);
m[1] = -m[1];
m[2] = -m[2];
Point p = (d / 2) * m;
double rx2 = _rays[X] * _rays[X];
double ry2 = _rays[Y] * _rays[Y];
double rxpy = _rays[X] * p[Y];
double rypx = _rays[Y] * p[X];
double rx2py2 = rxpy * rxpy;
double ry2px2 = rypx * rypx;
double num = rx2 * ry2;
double den = rx2py2 + ry2px2;
assert(den != 0);
double rad = num / den;
Point c(0,0);
if (rad > 1)
{
rad -= 1;
rad = std::sqrt(rad);
if (_large_arc == _sweep) rad = -rad;
c = rad * Point(rxpy / ray(Y), -rypx / ray(X));
_center = c * rm + middle_point(initialPoint(), finalPoint());
}
else if (rad == 1 || svg)
{
double lamda = std::sqrt(1 / rad);
_rays[X] *= lamda;
_rays[Y] *= lamda;
_center = middle_point(initialPoint(), finalPoint());
}
else
{
THROW_RANGEERROR(
"there is no ellipse that satisfies the given constraints"
);
}
Point sp((p[X] - c[X]) / ray(X), (p[Y] - c[Y]) / ray(Y));
Point ep((-p[X] - c[X]) / ray(X), (-p[Y] - c[Y]) / ray(Y));
Point v(1, 0);
_start_angle = angle_between(v, sp);
double sweep_angle = angle_between(sp, ep);
if (!_sweep && sweep_angle > 0) sweep_angle -= 2*M_PI;
if (_sweep && sweep_angle < 0) sweep_angle += 2*M_PI;
_end_angle = _start_angle;
_end_angle += sweep_angle;
}
std::vector<Coord> EllipticalArc::roots(Coord v, Dim2 d) const
{
std::vector<Coord> sol;
if ( are_near(ray(X), 0) && are_near(ray(Y), 0) ) {
if ( center(d) == v )
sol.push_back(0);
return sol;
}
static const char* msg[2][2] =
{
{ "d == X; ray(X) == 0; "
"s = (v - center(X)) / ( -ray(Y) * std::sin(_rot_angle) ); "
"s should be contained in [-1,1]",
"d == X; ray(Y) == 0; "
"s = (v - center(X)) / ( ray(X) * std::cos(_rot_angle) ); "
"s should be contained in [-1,1]"
},
{ "d == Y; ray(X) == 0; "
"s = (v - center(X)) / ( ray(Y) * std::cos(_rot_angle) ); "
"s should be contained in [-1,1]",
"d == Y; ray(Y) == 0; "
"s = (v - center(X)) / ( ray(X) * std::sin(_rot_angle) ); "
"s should be contained in [-1,1]"
},
};
for ( unsigned int dim = 0; dim < 2; ++dim )
{
if ( are_near(ray((Dim2) dim), 0) )
{
if ( initialPoint()[d] == v && finalPoint()[d] == v )
{
THROW_INFINITESOLUTIONS(0);
}
if ( (initialPoint()[d] < finalPoint()[d])
&& (initialPoint()[d] > v || finalPoint()[d] < v) )
{
return sol;
}
if ( (initialPoint()[d] > finalPoint()[d])
&& (finalPoint()[d] > v || initialPoint()[d] < v) )
{
return sol;
}
double ray_prj = 0.0;
switch(d)
{
case X:
switch(dim)
{
case X: ray_prj = -ray(Y) * std::sin(_rot_angle);
break;
case Y: ray_prj = ray(X) * std::cos(_rot_angle);
break;
}
break;
case Y:
switch(dim)
{
case X: ray_prj = ray(Y) * std::cos(_rot_angle);
break;
case Y: ray_prj = ray(X) * std::sin(_rot_angle);
break;
}
break;
}
double s = (v - center(d)) / ray_prj;
if ( s < -1 || s > 1 )
{
THROW_LOGICALERROR(msg[d][dim]);
}
switch(dim)
{
case X:
s = std::asin(s); // return a value in [-PI/2,PI/2]
if ( logical_xor( _sweep, are_near(initialAngle(), M_PI/2) ) )
{
if ( s < 0 ) s += 2*M_PI;
}
else
{
s = M_PI - s;
if (!(s < 2*M_PI) ) s -= 2*M_PI;
}
break;
case Y:
s = std::acos(s); // return a value in [0,PI]
if ( logical_xor( _sweep, are_near(initialAngle(), 0) ) )
{
s = 2*M_PI - s;
if ( !(s < 2*M_PI) ) s -= 2*M_PI;
}
break;
}
//std::cerr << "s = " << rad_to_deg(s);
s = map_to_01(s);
//std::cerr << " -> t: " << s << std::endl;
if ( !(s < 0 || s > 1) )
sol.push_back(s);
return sol;
}
}
double rotx, roty;
sincos(_rot_angle, roty, rotx);
if (d == X) roty = -roty;
double rxrotx = ray(X) * rotx;
double c_v = center(d) - v;
double a = -rxrotx + c_v;
double b = ray(Y) * roty;
double c = rxrotx + c_v;
//std::cerr << "a = " << a << std::endl;
//std::cerr << "b = " << b << std::endl;
//std::cerr << "c = " << c << std::endl;
if ( are_near(a,0) )
{
sol.push_back(M_PI);
if ( !are_near(b,0) )
{
double s = 2 * std::atan(-c/(2*b));
if ( s < 0 ) s += 2*M_PI;
sol.push_back(s);
}
}
else
{
double delta = b * b - a * c;
//std::cerr << "delta = " << delta << std::endl;
if ( are_near(delta, 0) )
{
double s = 2 * std::atan(-b/a);
if ( s < 0 ) s += 2*M_PI;
sol.push_back(s);
}
else if ( delta > 0 )
{
double sq = std::sqrt(delta);
double s = 2 * std::atan( (-b - sq) / a );
if ( s < 0 ) s += 2*M_PI;
sol.push_back(s);
s = 2 * std::atan( (-b + sq) / a );
if ( s < 0 ) s += 2*M_PI;
sol.push_back(s);
}
}
std::vector<double> arc_sol;
for (unsigned int i = 0; i < sol.size(); ++i )
{
//std::cerr << "s = " << rad_to_deg(sol[i]);
sol[i] = map_to_01(sol[i]);
//std::cerr << " -> t: " << sol[i] << std::endl;
if ( !(sol[i] < 0 || sol[i] > 1) )
arc_sol.push_back(sol[i]);
}
return arc_sol;
}
int main(int argc, char ** argv)
{
const std::string patchParamsFilename = argv[1];
const std::string testDataFilename = argv[2];
const std::string valveDataFilename = "/home/om0000/ValveTracking/TrainingData/MV-Planes.hdf5";
PatchParams params(patchParamsFilename);
TestData::Pointer testData = TestData::Initialise(testDataFilename);
LengthData::Pointer lengthData = LengthData::Load("/home/om0000/ValveTracking/TrainingData/Lengths");
lengthData->Prepare(testData->GetId());
std::cout << "Loading Plane Data" << std::endl;
ValveTrainingData::Pointer valveData = ValveTrainingData::Load(valveDataFilename);
std::cout << "Loading PatchData" << std::endl;
//PatchTrainingData::Pointer patchData = PatchTrainingData::Load(patchParamsFilename);
PatchTrainingData::Pointer patchData = PatchTrainingData::New();
VectorType startPoint, startNormal;
for(unsigned int runs = 0; runs < 1; runs++)
{
for(params.timeStep = 0; params.timeStep < params.numberOfTimeSteps; params.timeStep++)
{
std::cout << "Working on: " << params.timeStep << std::endl;
// load the input points and create the bounding box
MatrixType pointsData;
valveData->GetTrainingPoints(testData->GetId(), params.timeStep, pointsData);
std::vector<MatrixType> pointsData2;
valveData->GetTrainingPoints(testData->GetId(), params.timeStep, pointsData2);
vtkBoundingBox boundingBox;
computeBoundingBox(pointsData, boundingBox);
std::cout << "Loading / Training Classifiers" << std::endl;
ClassifierMap classifiers;
loadOrTrainClassifiers(params, 1000, patchData, classifiers);
testData->Prepare(params.timeStep, boundingBox);
testData->SaveImageGroup("2C", "MV-2C");
// compute the distribution for the plane and get the mean for initialisation
MatrixType planeData;
valveData->GetTrainingData(testData->GetId(), params.timeStep, planeData);
PlaneCovarianceFilterType::Pointer planeDistribution = PlaneCovarianceFilterType::New();
computePlaneCovariance(planeData, planeDistribution);
TestData::VectorType point, normal;
if(params.timeStep == 0 && runs == 0)
{
for(unsigned int i = 0; i < 3; i++)
{
point(i) = planeDistribution->GetMean()[i];
normal(i) = planeDistribution->GetMean()[i+3];
}
//startingPlane(pointsData2, point, normal);
}
else
{
point = startPoint;
normal = startNormal;
}
OptData * optData = new OptData;
optData->testData = testData;
optData->classifiers = classifiers;
optData->params = params;
optData->lengths = lengthData;
optData->planeMember = planeDistribution;
nlopt::opt opt(nlopt::LN_BOBYQA, 5);
opt.set_min_objective(f, (void*) optData);
opt.set_xtol_rel(1e-4);
opt.set_maxeval(100);
std::vector<double> initStep(5);
double pointStep = atof(argv[3]);
double normalStep = atof(argv[4]);
for(unsigned int i = 0; i < 3; i++)
{
initStep[i] = pointStep;
}
initStep[3] = normalStep;
initStep[4] = normalStep;
opt.set_default_initial_step(initStep);
std::vector<double> xStart;
convert_plane(point, normal, xStart);
double minf;
nlopt::result result = opt.optimize(xStart,minf);
std::cout << "Final: " << result << std::endl;
MatrixType initialPlane(1,6), finalPlane(1,6);
VectorType finalPoint, finalNormal;
convert_x(xStart, finalPoint, finalNormal);
imageCost(params, testData, finalPoint, finalNormal, classifiers, lengthData, true);
for(unsigned int i = 0; i < 3; i++)
{
finalPlane(0,i) = finalPoint(i);
finalPlane(0,i+3) = finalNormal(i);
}
std::cout << finalPlane << std::endl;
for(unsigned int i = 0; i < 6; i++)
{
initialPlane(0,i) = planeDistribution->GetMean()[i];
}
startPoint = finalPoint;
startNormal = finalNormal;
MatrixType gt;
valveData->GetTestData(testData->GetId(), params.timeStep, gt);
VectorType gtPoint, gtNormal;
for(unsigned int i = 0; i < 3; i++)
{
gtPoint(i) = gt(0,i);
gtNormal(i) = gt(0,i+3);
}
std::vector<ImageType::Pointer> vimages;
testData->GetImages(vimages);
ResultViewer::Pointer viewer = ResultViewer::New();
viewer->SetImages(vimages);
viewer->SetBoundingBox(boundingBox);
viewer->SetPlane(finalPoint, finalNormal);
viewer->SetStartPlane(gtPoint, gtNormal);
viewer->SetUp();
std::stringstream ss;
ss << params.name << "/" << testData->GetId() << "-" << params.timeStep << "-image.png";
viewer->Save(ss.str());
//viewer->View();
//testData->SaveImageGroup("2C", "2C");
//testData->SaveImageGroup("3C", "3C");
//testData->SaveImageGroup("4C", "4C");
//savePlane(initialPlane, "initialPlane.vtk");
//savePlane(finalPlane, "finalPlane.vtk");
}
}
return 0;
}
