}
// Projects v onto the convex polytope defined by linear inequalities Ax <= b.
-// Returns argmin_{x: Ax <= b} |x - v|_2. Uses Diykstra's (not Dijkstra's!)
-// projection algorithm [1].
+// Returns argmin_{x: Ax <= b} |x - v|_2. Uses Dykstra's (not Dijkstra's!)
+// projection algorithm [1] with robust stopping criteria [2].
//
// Args:
// v: vector to project into the polytope.
// A: n x 3 matrix, row-major. Each row must have L2 norm of 1.
// b: n vector.
// work: n x 3 matrix. will be overwritten. input values are not used.
-// tolerance: Halt once iterates satisfy |x_{k + 1} - x_k|_2 < tolerance.
-// Roughly, but not actually, equivalent to allowing constraint violations
-// up to Ax >= b + tolerance.
+// tolerance: Stop when *approximately* violates the polytope constraints
+// by no more than this value. Not exact - be conservative if needed.
+// maxiters: Terminate after this many iterations regardless of convergence.
//
// Returns:
// The projection of v into the polytope.
// [1] Boyle, J. P., and Dykstra, R. L. (1986). A Method for Finding
// Projections onto the Intersection of Convex Sets in Hilbert Spaces.
// Lecture Notes in Statistics, 28–47. doi:10.1007/978-1-4613-9940-7_3
+// [2] Birgin, E. G., and Raydan, M. (2005). Robust Stopping Criteria for
+// Dykstra's Algorithm. SIAM J. Scientific Computing 26(4): 1405-1414.
+// doi:10.1137/03060062X
//
-static inline struct vec vprojectpolytope(struct vec v, float const A[], float const b[], float work[], int n, float tolerance)
+static inline struct vec vprojectpolytope(struct vec v, float const A[], float const b[], float work[], int n, float tolerance, int maxiters)
{
// early bailout.
if (vinpolytope(v, A, b, n, tolerance)) {
z[i] = 0.0f;
}
- float const tolerance2 = fsqr(tolerance);
+ // For user-friendliness, we accept a tolerance value in terms of
+ // the Euclidean magnitude of the polytope violation. However, the
+ // stopping criteria we wish to use is the robust one of [2] -
+ // specifically, the expression called c_I^k - which is based on the
+ // sum of squared projection residuals. This is a feeble attempt to get
+ // a ballpark tolerance value that is roughly equivalent.
+ float const tolerance2 = n * fsqr(tolerance) / 10.0f;
struct vec x = v;
- while (true) {
- struct vec x_last_iter = x;
+ for (int iter = 0; iter < maxiters; ++iter) {
+ float c = 0.0f;
for (int i = 0; i < n; ++i) {
struct vec x_old = x;
struct vec ai = vloadf(A + 3 * i);
- struct vec zi = vloadf(z + 3 * i);
- x = vprojecthalfspace(vadd(x_old, zi), ai, b[i]);
- struct vec zi_new = vadd3(x_old, zi, vneg(x));
- vstoref(zi_new, z + 3 * i);
+ struct vec zi_old = vloadf(z + 3 * i);
+ x = vprojecthalfspace(vsub(x_old, zi_old), ai, b[i]);
+ struct vec zi = vadd3(x, vneg(x_old), zi_old);
+ vstoref(zi, z + 3 * i);
+ c += vdist2(zi_old, zi);
}
- if (vdist2(x_last_iter, x) < tolerance2) {
+ if (c < tolerance2) {
return x;
}
}
+ return x;
}
return mkvec(randu(-1.0f, 1.0f), randu(-1.0f, 1.0f), randu(-1.0f, 1.0f));
}
+// returns a random vector sampled from a Gaussian with expected L2 norm of 1.
+// Note that this is *not* an identity covariance matrix.
+struct vec randnvec() {
+ struct vec v = mkvec(randn(), randn(), randn());
+ return vscl(1.0 / sqrtf(3.0), v);
+}
+
// returns a random vector approximately uniformly sampled from the unit sphere
struct vec randsphere() {
struct vec v = mkvec(randn(), randn(), randn());
} \
} while(0) \
+// Generates n linear inequalities Ax <= b representing a convex polytope.
+//
+// The rows of A will be normalized to have L2 norm of 1.
+// The polytope interior will be non-empty.
+// The polytope may or may not contain the origin.
+// The polytope may or may not be bounded, but will be with high probability
+// as n increases.
+// The returned interior point will be, on average, around 1 unit away
+// from any polytope face.
+//
+// Returns a point in the polytope interior.
+struct vec randpolytope(float A[], float b[], int n) {
+ // If we generate random Ax <= b with b always positive, we ensure that
+ // x = 0 is always a solution; hence the polytope is non-empty. However,
+ // we also want to test nonempty polytopes that don't contain x = 0.
+ // Therefore, we use A(x - shift) <= b with b positive, which is
+ // equivalent to Ax <= b + Ashift.
+ struct vec shift = vscl(randu(0.0f, 4.0f), randsphere());
+
+ for (int i = 0; i < n; ++i) {
+ struct vec a = randsphere();
+ vstoref(a, A + 3 * i);
+ b[i] = randu(0.01f, 2.0f) + vdot(a, shift);
+ }
+
+ return shift;
+}
+
//
// tests - when adding new tests, make sure to add to test_fns array
printf("%s passed\n", __func__);
}
-void test_polytope()
+void test_polytope_projection()
{
srand(1); // deterministic
// and check that the projections are closer than other points.
int const N_POLYTOPES = 100;
int const N_PROJECTIONS = 10;
- int const N_OTHERS = 1000;
+ int const N_OTHERS = 100;
+ float const TOLERANCE = 1e-6;
+ float const MAXITERS = 500;
int const MAX_FACES = 30;
float *A = malloc(sizeof(float) * MAX_FACES * 3);
// Random number of polytope faces.
int n = rand() % MAX_FACES + 1;
- // If we generate random Ax <= b with b always positive, we ensure that
- // x = 0 is always a solution; hence the polytope is non-empty. However,
- // we also want to test nonempty polytopes that don't contain x = 0.
- // Therefore, we use A(x + shift) <= b with b positive, which is
- // equivalent to Ax <= b - Ashift.
- struct vec shift = randsphere();
-
- for (int i = 0; i < n; ++i) {
- struct vec a = randsphere();
- vstoref(a, A + 3 * i);
- b[i] = randu(0.01, 10) - vdot(a, shift);
- }
+ struct vec interior = randpolytope(A, b, n);
+ assert(vinpolytope(interior, A, b, n, 1e-10));
for (int point = 0; point < N_PROJECTIONS; ++point) {
+
struct vec x = randsphere();
- struct vec xp = vprojectpolytope(x, A, b, work, n, 1e-6);
+ struct vec xp = vprojectpolytope(x, A, b, work, n, TOLERANCE, MAXITERS);
// Feasibility check: projection is inside polytope.
// The tolerance is looser than the vprojectpolytope tolerance
// because that tolerance doesn't actually guarantee a rigid bound
// on constraint violations.
- assert(vinpolytope(xp, A, b, n, 1e-5));
+ assert(vinpolytope(xp, A, b, n, 10 * TOLERANCE));
// Optimality check: projection is closer than other random points
// to query point. Very large N_OTHERS would be more thorough...
for (int other = 0; other < N_OTHERS; ++other) {
- struct vec other = vadd(randsphere(), shift);
+ struct vec other = vadd(randnvec(), interior);
if (vinpolytope(other, A, b, n, 0.0f)) {
- assert (vdist2(xp, x) <= vdist2(other, x));
+ assert(vdist2(xp, x) <= vdist2(other, x));
}
}
}
printf("%s passed\n", __func__);
}
+
// micro test framework
typedef void (*voidvoid_fn)(void);
voidvoid_fn test_fns[] = {
test_quat_rpy_conversions,
test_quat_mat_conversions,
test_qvectovec,
- test_polytope,
+ test_polytope_projection,
};
static int i_test = -1;
projects / forks / cmath3d.git / commitdiff