Changeset 5745 for trunk/stac/src/stacInvertMaps.c
- Timestamp:
- Dec 7, 2005, 4:04:22 PM (21 years ago)
- File:
-
- 1 edited
-
trunk/stac/src/stacInvertMaps.c (modified) (2 diffs)
Legend:
- Unmodified
- Added
- Removed
-
trunk/stac/src/stacInvertMaps.c
r5743 r5745 9 9 10 10 psArray *stacInvertMaps(const psArray *maps, // Array of maps to invert 11 11 int outnx, int outny // Size of output image 12 12 ) 13 13 { 14 int nMaps = maps->n; 14 int nMaps = maps->n; // Number of maps 15 15 psArray *inverted = psArrayAlloc(nMaps); // Array of inverted maps for output 16 16 … … 23 23 for (int mapNum = 0; mapNum < nMaps; mapNum++) { 24 24 25 26 27 28 29 int order = oldMap->x->nX + 1;// Polynomial order30 31 psPlaneTransform *newMap = psPlaneTransformAlloc(order + 1, order + 1);// Inverted map32 33 34 35 for (int i = 0; i <order; i++) {36 for (int j = 0; j <order; j++) {37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 int nCoeff = order * (order + 1) / 2; // Number of polynomial coefficients65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 for (int i = 0, ijIndex = 0; i <order; i++) {81 for (int j = 0; j <order - i; j++, ijIndex++) {82 83 84 85 86 87 for (int m = 0, mnIndex = 0; m <order; m++) {88 for (int n = 0; n <order - m; n++, mnIndex++) {89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 for (int i = 0, ijIndex = 0; i <order; i++) {112 for (int j = 0; j <order - i; j++, ijIndex++) {113 114 115 116 117 25 psPlaneTransform *oldMap = (psPlaneTransform*)maps->data[mapNum]; // Uninverted map 26 // Check input 27 assert(oldMap->x->nX == oldMap->x->nY && oldMap->y->nX == oldMap->y->nY && 28 oldMap->x->nX == oldMap->y->nX); 29 int order = oldMap->x->nX; // Polynomial order 30 psTrace("stac.invertMaps", 4, "Generating order %d polynomial inverse transformation.\n", order); 31 psPlaneTransform *newMap = psPlaneTransformAlloc(order, order); // Inverted map 32 33 // Create fake polynomial to use in evaluation 34 psPolynomial2D *fakePoly = psPolynomial2DAlloc(order, order, PS_POLYNOMIAL_ORD); 35 for (int i = 0; i <= order; i++) { 36 for (int j = 0; j <= order; j++) { 37 fakePoly->coeff[i][j] = 1.0; // Set all coeffecients to 1 38 fakePoly->mask[i][j] = 1; // Mask all coefficients; unmask to evaluate 39 } 40 } 41 42 // A grid of xin,yin --> xout,yout 43 psVector *xIn = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32); 44 psVector *yIn = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32); 45 psVector *xOut = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32); 46 psVector *yOut = psVectorAlloc(NUM_GRID * NUM_GRID, PS_TYPE_F32); 47 48 // Create grid of points 49 for (int yint = 0; yint < NUM_GRID; yint++) { 50 inCoord->y = (float)(yint * outny) / (float)(NUM_GRID - 1); 51 for (int xint = 0; xint < NUM_GRID; xint++) { 52 inCoord->x = (float)(xint * outnx) / (float)(NUM_GRID - 1); 53 54 (void)psPlaneTransformApply(outCoord, oldMap, inCoord); 55 56 xOut->data.F32[yint*NUM_GRID + xint] = inCoord->x; 57 yOut->data.F32[yint*NUM_GRID + xint] = inCoord->y; 58 xIn->data.F32[yint*NUM_GRID + xint] = outCoord->x; 59 yIn->data.F32[yint*NUM_GRID + xint] = outCoord->y; 60 } 61 } 62 63 // Initialise the matrix and vectors 64 int nCoeff = (order + 1) * (order + 2) / 2; // Number of polynomial coefficients 65 psImage *matrix = psImageAlloc(nCoeff, nCoeff, PS_TYPE_F64); // Matrix for solution 66 psVector *xVector = psVectorAlloc(nCoeff, PS_TYPE_F64); // Vector for solution in x 67 psVector *yVector = psVectorAlloc(nCoeff, PS_TYPE_F64); // Vector for solution in y 68 for (int i = 0; i < nCoeff; i++) { 69 for (int j = 0; j < nCoeff; j++) { 70 matrix->data.F64[i][j] = 0.0; 71 } 72 xVector->data.F64[i] = 0.0; 73 yVector->data.F64[i] = 0.0; 74 } 75 76 // Iterate over the grid points 77 for (int g = 0; g < NUM_GRID*NUM_GRID; g++) { 78 79 // Iterate over the polynomial coefficients, accumulating the matrix and vectors 80 for (int i = 0, ijIndex = 0; i <= order; i++) { 81 for (int j = 0; j <= order - i; j++, ijIndex++) { 82 83 fakePoly->mask[i][j] = 0; 84 double ijPoly = psPolynomial2DEval(fakePoly, xIn->data.F32[g], yIn->data.F32[g]); 85 fakePoly->mask[i][j] = 1; 86 87 for (int m = 0, mnIndex = 0; m <= order; m++) { 88 for (int n = 0; n <= order - m; n++, mnIndex++) { 89 90 fakePoly->mask[m][n] = 0; 91 double mnPoly = psPolynomial2DEval(fakePoly, xIn->data.F32[g], yIn->data.F32[g]); 92 fakePoly->mask[m][n] = 1; 93 94 matrix->data.F64[ijIndex][mnIndex] += ijPoly * mnPoly; 95 } 96 } 97 98 xVector->data.F64[ijIndex] += ijPoly * (double)xOut->data.F32[g]; 99 yVector->data.F64[ijIndex] += ijPoly * (double)yOut->data.F32[g]; 100 } 101 } // Iterating over coefficients 102 } // Iterating over grid points 103 104 // Solution via LU Decomposition 105 psVector *permutation = psVectorAlloc(nCoeff, PS_TYPE_F64); // Permutation vector for LU Decomposition 106 psImage *luMatrix = psMatrixLUD(NULL, &permutation, matrix); // LU decomposed matrix 107 psVector *xSolution = psMatrixLUSolve(NULL, luMatrix, xVector, permutation); // Solution in x 108 psVector *ySolution = psMatrixLUSolve(NULL, luMatrix, yVector, permutation); // Solution in y 109 110 // Stuff coefficients into transformation 111 for (int i = 0, ijIndex = 0; i <= order; i++) { 112 for (int j = 0; j <= order - i; j++, ijIndex++) { 113 newMap->x->coeff[i][j] = xSolution->data.F64[ijIndex]; 114 newMap->y->coeff[i][j] = ySolution->data.F64[ijIndex]; 115 } 116 } 117 inverted->data[mapNum] = newMap; 118 118 119 119 #ifdef TESTING 120 121 122 for (int i = 0; i <order; i++) {123 for (int j = 0; j <order - i; j++) {124 125 126 127 128 129 for (int i = 0; i <order; i++) {130 for (int j = 0; j <order - i; j++) {131 132 133 120 // Print x coefficients 121 psTrace("stac.invertMaps", 7, "x' = \n"); 122 for (int i = 0; i <= order; i++) { 123 for (int j = 0; j <= order - i; j++) { 124 psTrace("stac.invertMaps", 7, " %f x^%d y^%d\n", newMap->x->coeff[i][j], i, j); 125 } 126 } 127 // Print y coefficients 128 psTrace("stac.invertMaps", 7, "y' = \n"); 129 for (int i = 0; i <= order; i++) { 130 for (int j = 0; j <= order - i; j++) { 131 psTrace("stac.invertMaps", 7, " %f x^%d y^%d\n", newMap->y->coeff[i][j], i, j); 132 } 133 } 134 134 #endif 135 135 136 136 #ifdef TESTING 137 138 139 140 141 142 143 144 145 146 147 148 137 // Go forward then backward 138 double x = 123.4; 139 double y = 432.1; 140 psPlane *oldCoords = psAlloc(sizeof(psPlane)); 141 oldCoords->x = x; 142 oldCoords->y = y; 143 psPlane *newCoords = psPlaneTransformApply(NULL, oldMap, oldCoords); 144 psTrace("stac.invertMaps.test", 5, "%f,%f --> %f,%f\n", x, y, newCoords->x, newCoords->y); 145 (void)psPlaneTransformApply(oldCoords, newMap, newCoords); 146 psTrace("stac.invertMaps.test", 5, "--------> %f,%f\n", oldCoords->x, oldCoords->y); 147 psFree(newCoords); 148 psFree(oldCoords); 149 149 #endif 150 150 151 152 153 154 155 156 157 158 159 160 161 162 151 psFree(permutation); 152 psFree(luMatrix); 153 psFree(matrix); 154 psFree(xVector); 155 psFree(yVector); 156 psFree(xSolution); 157 psFree(ySolution); 158 psFree(fakePoly); 159 psFree(xIn); 160 psFree(yIn); 161 psFree(xOut); 162 psFree(yOut); 163 163 } 164 164 165 165 psFree(inCoord); 166 166 psFree(outCoord); 167 167 168 168 169 169 #if 0 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 170 // Can't handle higher order than linear yet 171 assert(((psPlaneTransform*)maps->data[i])->x->nX == 2 && 172 ((psPlaneTransform*)maps->data[i])->x->nY == 2 && 173 ((psPlaneTransform*)maps->data[i])->y->nX == 2 && 174 ((psPlaneTransform*)maps->data[i])->y->nY == 2); 175 psPlaneTransform *newMap = psPlaneTransformAlloc(2, 2); // Inverted map 176 psPlaneTransform *oldMap = (psPlaneTransform*)maps->data[i]; // Uninverted map 177 178 // Now, simply do a 2x2 matrix inversion 179 180 double a = oldMap->x->coeff[1][0]; 181 double b = oldMap->x->coeff[0][1]; 182 double c = oldMap->y->coeff[1][0]; 183 double d = oldMap->y->coeff[0][1]; 184 double e = oldMap->x->coeff[0][0]; 185 double f = oldMap->y->coeff[0][0]; 186 187 double invDet = 1.0 / (a * d - b * c); // Inverse of the determinant 188 189 // Not entirely sure why this works, but it appears to do so....................................... 190 newMap->x->coeff[1][0] = invDet * a; 191 newMap->x->coeff[0][1] = - invDet * b; 192 newMap->y->coeff[1][0] = - invDet * c; 193 newMap->y->coeff[0][1] = invDet * d; 194 195 newMap->x->coeff[0][0] = - invDet * (d * e + c * f); 196 newMap->y->coeff[0][0] = - invDet * (b * e + a * f); 197 197 #endif 198 198
Note:
See TracChangeset
for help on using the changeset viewer.
