bool ConstraintBSpline::reduceVariableRanges() const
{
// Update bound of y in f(x) - y = 0
auto cp = bspline.getControlPoints();
DenseVector minControlPoints = cp.colwise().minCoeff();
DenseVector maxControlPoints = cp.colwise().maxCoeff();
for (unsigned int i = variables.size()-numConstraints; i < variables.size(); i++)
{
assert(minControlPoints(i) <= maxControlPoints(i));
// Fix for B-splines with collapsed bounds
double xlb = variables.at(i)->getLowerBound();
double xub = variables.at(i)->getUpperBound();
if (assertNear(xlb, xub))
continue;
double newlb = std::max(xlb, minControlPoints(i));
double newub = std::min(xub, maxControlPoints(i));
// Detect constraint infeasibility or update bounds
if (!variables.at(i)->updateBounds(newlb, newub))
return false;
}
// Compute and check variable bounds
return controlPointBoundsDeduction();
}
RowVectorXi findDelaunayTriangulation(Eigen::Ref<const RowVectorX> ileavedPoints)
{
auto points = toSeparatedViewConst<Scalar>(ileavedPoints);
eigen_assert(points.cols() == 2);
// Find min max.
RowVector2 minC = points.colwise().minCoeff();
RowVector2 maxC = points.colwise().maxCoeff();
// Don't make the bounds too tight.
cv::Rect_<float> bounds(
std::floor(minC.x() - aam::Scalar(1)),
std::floor(minC.y() - aam::Scalar(1)),
std::ceil(maxC.x() - minC.x() + aam::Scalar(2)),
std::ceil(maxC.y() - minC.y() + aam::Scalar(2)));
cv::Subdiv2D subdiv(bounds);
std::vector<cv::Point2f> controlPoints;
for (MatrixX::Index i = 0; i < points.rows(); ++i) {
cv::Point2f c(points(i, 0), points(i, 1));
subdiv.insert(c);
controlPoints.push_back(c);
}
std::vector<cv::Vec6f> triangleList;
subdiv.getTriangleList(triangleList);
RowVectorXi triangleIds(triangleList.size() * 3);
int validTris = 0;
for (size_t i = 0; i < triangleList.size(); i++)
{
cv::Vec6f t = triangleList[i];
cv::Point2f p0(t[0], t[1]);
cv::Point2f p1(t[2], t[3]);
cv::Point2f p2(t[4], t[5]);
if (bounds.contains(p0) && bounds.contains(p1) && bounds.contains(p2)) {
auto iter0 = std::find(controlPoints.begin(), controlPoints.end(), p0);
auto iter1 = std::find(controlPoints.begin(), controlPoints.end(), p1);
auto iter2 = std::find(controlPoints.begin(), controlPoints.end(), p2);
triangleIds(validTris * 3 + 0) = (int)std::distance(controlPoints.begin(), iter0);
triangleIds(validTris * 3 + 1) = (int)std::distance(controlPoints.begin(), iter1);
triangleIds(validTris * 3 + 2) = (int)std::distance(controlPoints.begin(), iter2);
++validTris;
}
}
return triangleIds.leftCols(validTris * 3);
}
MatrixXd individualSupportCOPs(RigidBodyManipulator* r, const std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>& active_supports,
const MatrixXd& normals, const MatrixXd& B, const VectorXd& beta)
{
const int n_basis_vectors_per_contact = static_cast<int>(B.cols() / normals.cols());
const int n = static_cast<int>(active_supports.size());
int normals_start = 0;
int beta_start = 0;
MatrixXd individual_cops(3, n);
individual_cops.fill(std::numeric_limits<double>::quiet_NaN());
for (size_t j = 0; j < active_supports.size(); j++) {
auto active_support = active_supports[j];
auto contact_pts = active_support.contact_pts;
int ncj = static_cast<int>(contact_pts.size());
int active_support_length = n_basis_vectors_per_contact * ncj;
auto normalsj = normals.middleCols(normals_start, ncj);
Vector3d normal = normalsj.col(0);
bool normals_identical = (normalsj.colwise().operator-(normal)).squaredNorm() < 1e-15;
if (normals_identical) { // otherwise computing a COP doesn't make sense
const auto& Bj = B.middleCols(beta_start, active_support_length);
const auto& betaj = beta.segment(beta_start, active_support_length);
const auto& contact_positions = r->bodies[active_support.body_idx]->contact_pts;
Vector3d force = Vector3d::Zero();
Vector3d torque = Vector3d::Zero();
for (size_t k = 0; k < contact_pts.size(); k++) {
// for (auto k = contact_pts.begin(); k!= contact_pts.end(); k++) {
const auto& Bblock = Bj.middleCols(k * n_basis_vectors_per_contact, n_basis_vectors_per_contact);
const auto& betablock = betaj.segment(k * n_basis_vectors_per_contact, n_basis_vectors_per_contact);
Vector3d point_force = Bblock * betablock;
force += point_force;
Vector3d contact_pt = contact_pts[k].head(3);
auto torquejk = contact_pt.cross(point_force);
torque += torquejk;
}
Vector3d point_on_contact_plane = contact_positions.col(0);
std::pair<Vector3d, double> cop_and_normal_torque = resolveCenterOfPressure(torque, force, normal, point_on_contact_plane);
Vector4d cop_body;
cop_body << cop_and_normal_torque.first, 1.0;
Vector3d cop_world;
r->forwardKin(active_support.body_idx, cop_body, 0, cop_world);
individual_cops.col(j) = cop_world;
}
normals_start += ncj;
beta_start += active_support_length;
}
return individual_cops;
}
ConstraintPtr ConstraintBSpline::computeRelaxationHyperrectangle()
{
/*
* Hyperrectangle model:
*
* X = [x' y']'
*
* A = [-I ] b = [-lb ]
* [ I ] [ ub ]
*
* AX <= b
*/
int dim = bspline.getNumVariables() + 1;
assert(dim == (int)variables.size());
auto cp = bspline.getControlPoints();
DenseVector minControlPoints = cp.colwise().minCoeff();
DenseVector maxControlPoints = cp.colwise().maxCoeff();
DenseMatrix Idim;
Idim.setIdentity(dim,dim);
DenseMatrix A(2*dim, dim);
A.block(0,0, dim, dim) = - Idim;
A.block(dim,0, dim, dim) = Idim;
DenseVector b(2*dim);
b.block(0, 0, dim, 1) = - minControlPoints;
b.block(dim, 0, dim, 1) = maxControlPoints;
ConstraintPtr relaxedConstraint = std::make_shared<ConstraintLinear>(variables, A ,b, false);
relaxedConstraint->setName("B-spline hypercube relaxation (linear)");
return relaxedConstraint;
}
static void CalculateFeatures(
mitk::GreyLevelSizeZoneMatrixHolder &holder,
mitk::GreyLevelSizeZoneFeatures & results
)
{
auto SgzMatrix = holder.m_Matrix;
auto pgzMatrix = holder.m_Matrix;
auto pgMatrix = holder.m_Matrix;
auto pzMatrix = holder.m_Matrix;
double Ns = pgzMatrix.sum();
pgzMatrix /= Ns;
pgMatrix.rowwise().normalize();
pzMatrix.colwise().normalize();
for (int i = 0; i < holder.m_NumberOfBins; ++i)
for (int j = 0; j < holder.m_NumberOfBins; ++j)
{
if (pgzMatrix(i, j) != pgzMatrix(i, j))
pgzMatrix(i, j) = 0;
if (pgMatrix(i, j) != pgMatrix(i, j))
pgMatrix(i, j) = 0;
if (pzMatrix(i, j) != pzMatrix(i, j))
pzMatrix(i, j) = 0;
}
Eigen::VectorXd SgVector = SgzMatrix.rowwise().sum();
Eigen::VectorXd SzVector = SgzMatrix.colwise().sum();
for (int j = 0; j < SzVector.size(); ++j)
{
results.SmallZoneEmphasis += SzVector(j) / (j + 1) / (j + 1);
results.LargeZoneEmphasis += SzVector(j) * (j + 1.0) * (j + 1.0);
results.ZoneSizeNonUniformity += SzVector(j) * SzVector(j);
results.ZoneSizeNoneUniformityNormalized += SzVector(j) * SzVector(j);
}
for (int i = 0; i < SgVector.size(); ++i)
{
results.LowGreyLevelEmphasis += SgVector(i) / (i + 1) / (i + 1);
results.HighGreyLevelEmphasis += SgVector(i) * (i + 1) * (i + 1);
results.GreyLevelNonUniformity += SgVector(i)*SgVector(i);
results.GreyLevelNonUniformityNormalized += SgVector(i)*SgVector(i);
}
for (int i = 0; i < SgzMatrix.rows(); ++i)
{
for (int j = 0; j < SgzMatrix.cols(); ++j)
{
results.SmallZoneLowGreyLevelEmphasis += SgzMatrix(i, j) / (i + 1) / (i + 1) / (j + 1) / (j + 1);
results.SmallZoneHighGreyLevelEmphasis += SgzMatrix(i, j) * (i + 1) * (i + 1) / (j + 1) / (j + 1);
results.LargeZoneLowGreyLevelEmphasis += SgzMatrix(i, j) / (i + 1) / (i + 1) * (j + 1.0) * (j + 1.0);
results.LargeZoneHighGreyLevelEmphasis += SgzMatrix(i, j) * (i + 1) * (i + 1) * (j + 1.0) * (j + 1.0);
results.ZonePercentage += SgzMatrix(i, j)*(j + 1);
results.GreyLevelMean += (i + 1)*pgzMatrix(i, j);
results.ZoneSizeMean += (j + 1)*pgzMatrix(i, j);
if (pgzMatrix(i, j) > 0)
results.ZoneSizeEntropy -= pgzMatrix(i, j) * std::log(pgzMatrix(i, j)) / std::log(2);
}
}
for (int i = 0; i < SgzMatrix.rows(); ++i)
{
for (int j = 0; j < SgzMatrix.cols(); ++j)
{
results.GreyLevelVariance += (i + 1 - results.GreyLevelMean)*(i + 1 - results.GreyLevelMean)*pgzMatrix(i, j);
results.ZoneSizeVariance += (j + 1 - results.ZoneSizeMean)*(j + 1 - results.ZoneSizeMean)*pgzMatrix(i, j);
}
}
results.SmallZoneEmphasis /= Ns;
results.LargeZoneEmphasis /= Ns;
results.LowGreyLevelEmphasis /= Ns;
results.HighGreyLevelEmphasis /= Ns;
results.SmallZoneLowGreyLevelEmphasis /= Ns;
results.SmallZoneHighGreyLevelEmphasis /= Ns;
results.LargeZoneLowGreyLevelEmphasis /= Ns;
results.LargeZoneHighGreyLevelEmphasis /= Ns;
results.GreyLevelNonUniformity /= Ns;
results.GreyLevelNonUniformityNormalized /= Ns*Ns;
results.ZoneSizeNonUniformity /= Ns;
results.ZoneSizeNoneUniformityNormalized /= Ns*Ns;
results.ZonePercentage = Ns / results.ZonePercentage;
}
// apply the distmesh algorithm
std::tuple<Eigen::ArrayXXd, Eigen::ArrayXXi> distmesh::distmesh(
Functional const& distanceFunction, double const initialPointDistance,
Functional const& elementSizeFunction, Eigen::Ref<Eigen::ArrayXXd const> const boundingBox,
Eigen::Ref<Eigen::ArrayXXd const> const fixedPoints) {
// determine dimension of mesh
unsigned const dimension = boundingBox.cols();
// create initial distribution in bounding box
Eigen::ArrayXXd points = utils::createInitialPoints(distanceFunction,
initialPointDistance, elementSizeFunction, boundingBox, fixedPoints);
// create initial triangulation
Eigen::ArrayXXi triangulation = triangulation::delaunay(points);
// create buffer to store old point locations to calculate
// retriangulation and stop criterion
Eigen::ArrayXXd retriangulationCriterionBuffer = Eigen::ArrayXXd::Constant(
points.rows(), points.cols(), INFINITY);
Eigen::ArrayXXd stopCriterionBuffer = Eigen::ArrayXXd::Zero(
points.rows(), points.cols());
// main distmesh loop
Eigen::ArrayXXi edgeIndices;
for (unsigned step = 0; step < constants::maxSteps; ++step) {
// retriangulate if point movement is above threshold
if ((points - retriangulationCriterionBuffer).square().rowwise().sum().sqrt().maxCoeff() >
constants::retriangulationThreshold * initialPointDistance) {
// update triangulation
triangulation = triangulation::delaunay(points);
// reject triangles with circumcenter outside of the region
Eigen::ArrayXXd circumcenter = Eigen::ArrayXXd::Zero(triangulation.rows(), dimension);
for (int point = 0; point < triangulation.cols(); ++point) {
circumcenter += utils::selectIndexedArrayElements<double>(
points, triangulation.col(point)) / triangulation.cols();
}
triangulation = utils::selectMaskedArrayElements<int>(triangulation,
distanceFunction(circumcenter) < -constants::geometryEvaluationThreshold * initialPointDistance);
// find unique edge indices
edgeIndices = utils::findUniqueEdges(triangulation);
// store current points positions
retriangulationCriterionBuffer = points;
}
// calculate edge vectors and their length
auto const edgeVector = (utils::selectIndexedArrayElements<double>(points, edgeIndices.col(0)) -
utils::selectIndexedArrayElements<double>(points, edgeIndices.col(1))).eval();
auto const edgeLength = edgeVector.square().rowwise().sum().sqrt().eval();
// evaluate elementSizeFunction at midpoints of edges
auto const desiredElementSize = elementSizeFunction(0.5 *
(utils::selectIndexedArrayElements<double>(points, edgeIndices.col(0)) +
utils::selectIndexedArrayElements<double>(points, edgeIndices.col(1)))).eval();
// calculate desired edge length
auto const desiredEdgeLength = (desiredElementSize * (1.0 + 0.4 / std::pow(2.0, dimension - 1)) *
std::pow((edgeLength.pow(dimension).sum() / desiredElementSize.pow(dimension).sum()),
1.0 / dimension)).eval();
// calculate force vector for each edge
auto const forceVector = (edgeVector.colwise() *
((desiredEdgeLength - edgeLength) / edgeLength).max(0.0)).eval();
// store current points positions
stopCriterionBuffer = points;
// move points
for (int edge = 0; edge < edgeIndices.rows(); ++edge) {
if (edgeIndices(edge, 0) >= fixedPoints.rows()) {
points.row(edgeIndices(edge, 0)) += constants::deltaT * forceVector.row(edge);
}
if (edgeIndices(edge, 1) >= fixedPoints.rows()) {
points.row(edgeIndices(edge, 1)) -= constants::deltaT * forceVector.row(edge);
}
}
// project points outside of domain to boundary
utils::projectPointsToBoundary(distanceFunction, initialPointDistance, points);
// stop, when maximum points movement is below threshold
if ((points - stopCriterionBuffer).square().rowwise().sum().sqrt().maxCoeff() <
constants::pointsMovementThreshold * initialPointDistance) {
break;
}
}
return std::make_tuple(points, triangulation);
}