├── .gitignore ├── EigenSolver.h ├── InertiaCalculations.cpp ├── InertiaCalculations.h ├── Minimization.cpp ├── Minimization.h ├── OPECommon.h ├── OPEMain.cpp ├── OPESettings.cpp ├── OPESettings.h ├── OPEUtils.cpp ├── OPEUtils.h ├── ObjectPoseEstimation.vcxproj ├── ObjectPoseEstimation.vcxproj.filters ├── ObjectPoseEstimator.cpp ├── ObjectPoseEstimator.h ├── Plane.cpp ├── Plane.h ├── PointCloudCapture.cpp ├── PointCloudCapture.h ├── README.md ├── SQFitting.cpp ├── SQFitting.h ├── SQTypes.h ├── TableObjectDetector.h ├── TableObjectDetector.hpp └── TableObjectModeler.h /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | #ignore thumbnails created by windows 3 | Thumbs.db 4 | 5 | #Ignore files build by Visual Studio 6 | *.obj 7 | *.exe 8 | *.pdb 9 | *.user 10 | *.aps 11 | *.pch 12 | *.vspscc 13 | *_i.c 14 | *_p.c 15 | *.ncb 16 | *.suo 17 | *.tlb 18 | *.tlh 19 | *.bak 20 | *.cache 21 | *.ilk 22 | *.log 23 | [Bb]in 24 | [Dd]ebug*/ 25 | *.sbr 26 | ipch/ 27 | obj/ 28 | [Rr]elease*/ 29 | _ReSharper*/ 30 | [Tt]est[Rr]esult* 31 | .svn 32 | -------------------------------------------------------------------------------- /EigenSolver.h: -------------------------------------------------------------------------------- 1 | #ifndef __EIGENSOLVER_H 2 | #define __EIGENSOLVER_H 3 | 4 | /*/////////////////////////////////////////////////////////////////////////////////////// 5 | // 6 | // INTEL CORPORATION PROPRIETARY INFORMATION 7 | // This software is supplied under the terms of a license agreement or 8 | // nondisclosure agreement with Intel Corporation and may not be copied 9 | // or disclosed except in accordance with the terms of that agreement. 10 | // Copyright (c) 1999 Intel Corporation. All Rights Reserved. 11 | // 12 | // RCS: 13 | // $Source$ 14 | // $Revision: 208 $ 15 | // Purpose: 16 | // Contents: 17 | // Authors: 18 | // Dmitry Abrosimov 19 | //*/ 20 | 21 | #include 22 | 23 | /*/////////////////////////////////////////////////////////////////////////////////////// 24 | // Names: cvJacobiEigens_32f, cvJacobiEigens_64d 25 | // Purpose: Eigenvalues & eigenvectors calculation of a symmetric matrix: 26 | // A Vi = Ei Vi 27 | // Context: 28 | // Parameters: A(n, n) - source symmetric matrix (n - rows & columns number), 29 | // V(n, n) - matrix of its eigenvectors 30 | // (i-th row is an eigenvector Vi), 31 | // E(n) - vector of its eigenvalues 32 | // (i-th element is an eigenvalue Ei), 33 | // eps - accuracy of diagonalization. 34 | // 35 | // Returns: 36 | // CV_NO_ERROR or error code 37 | // Notes: 38 | // 1. The functions destroy source matrix A, so if you need it after 39 | // running cvJacobiEigens_???, you must copy it before. 40 | // 2. Eigenvalies and eigenvectors are sorted in Ei absolute value descending. 41 | // 3. Calculation time depends on eps value. If the time isn't very important, 42 | // we recommend to set eps = 0. 43 | //*/ 44 | 45 | /*=========================== Single precision function ================================*/ 46 | 47 | int cvJacobiEigens_32f(float* A, float* V, float* E, int n, float eps) { 48 | int i, j, k, ind; 49 | float *AA = A, *VV = V; 50 | double Amax, anorm = 0, ax; 51 | 52 | /*if ( A == NULL || V == NULL || E == NULL ) return CV_NULLPTR_ERR;*/ 53 | /*if ( n <= 0 ) return CV_BADSIZE_ERR;*/ 54 | if (eps < 1.0e-7f) 55 | eps = 1.0e-7f; 56 | 57 | /*-------- Prepare --------*/ 58 | for (i = 0; i < n; i++, VV += n, AA += n) { 59 | for (j = 0; j < i; j++) { 60 | double Am = AA[j]; 61 | anorm += Am * Am; 62 | } 63 | for (j = 0; j < n; j++) 64 | VV[j] = 0.f; 65 | VV[i] = 1.f; 66 | } 67 | 68 | anorm = sqrt(anorm + anorm); 69 | ax = anorm * eps / n; 70 | Amax = anorm; 71 | 72 | while (Amax > ax) { 73 | Amax /= n; 74 | do /* while (ind) */ 75 | { 76 | int p, q; 77 | float *V1 = V, *A1 = A; 78 | ind = 0; 79 | for (p = 0; p < n - 1; p++, A1 += n, V1 += n) { 80 | float * A2 = A + n * (p + 1), *V2 = V + n * (p + 1); 81 | for (q = p + 1; q < n; q++, A2 += n, V2 += n) { 82 | double x, y, c, s, c2, s2, a; 83 | float *A3, Apq = A1[q], App, Aqq, Aip, Aiq, Vpi, Vqi; 84 | if (fabs(Apq) < Amax) 85 | continue; 86 | 87 | ind = 1; 88 | 89 | /*---- Calculation of rotation angle's sine & cosine ----*/ 90 | App = A1[p]; 91 | Aqq = A2[q]; 92 | y = 5.0e-1 * (App - Aqq); 93 | x = -Apq / sqrt(Apq * Apq + y * y); 94 | if (y < 0.0) 95 | x = -x; 96 | s = x / sqrt(2.0 * (1.0 + sqrt(1.0 - x * x))); 97 | s2 = s * s; 98 | c = sqrt(1.0 - s2); 99 | c2 = c * c; 100 | a = 2.0 * Apq * c * s; 101 | 102 | /*---- Apq annulation ----*/ 103 | A3 = A; 104 | for (i = 0; i < p; i++, A3 += n) { 105 | Aip = A3[p]; 106 | Aiq = A3[q]; 107 | Vpi = V1[i]; 108 | Vqi = V2[i]; 109 | A3[p] = (float) (Aip * c - Aiq * s); 110 | A3[q] = (float) (Aiq * c + Aip * s); 111 | V1[i] = (float) (Vpi * c - Vqi * s); 112 | V2[i] = (float) (Vqi * c + Vpi * s); 113 | } 114 | for (; i < q; i++, A3 += n) { 115 | Aip = A1[i]; 116 | Aiq = A3[q]; 117 | Vpi = V1[i]; 118 | Vqi = V2[i]; 119 | A1[i] = (float) (Aip * c - Aiq * s); 120 | A3[q] = (float) (Aiq * c + Aip * s); 121 | V1[i] = (float) (Vpi * c - Vqi * s); 122 | V2[i] = (float) (Vqi * c + Vpi * s); 123 | } 124 | for (; i < n; i++) { 125 | Aip = A1[i]; 126 | Aiq = A2[i]; 127 | Vpi = V1[i]; 128 | Vqi = V2[i]; 129 | A1[i] = (float) (Aip * c - Aiq * s); 130 | A2[i] = (float) (Aiq * c + Aip * s); 131 | V1[i] = (float) (Vpi * c - Vqi * s); 132 | V2[i] = (float) (Vqi * c + Vpi * s); 133 | } 134 | A1[p] = (float) (App * c2 + Aqq * s2 - a); 135 | A2[q] = (float) (App * s2 + Aqq * c2 + a); 136 | A1[q] = A2[p] = 0.0f; 137 | } /*q*/ 138 | } /*p*/ 139 | } while (ind); 140 | Amax /= n; 141 | } /* while ( Amax > ax ) */ 142 | 143 | for (i = 0, k = 0; i < n; i++, k += n + 1) 144 | E[i] = A[k]; 145 | /*printf(" M = %d\n", M);*/ 146 | 147 | /* -------- ordering --------*/ 148 | for (i = 0; i < n; i++) { 149 | int m = i; 150 | float Em = (float) fabs(E[i]); 151 | for (j = i + 1; j < n; j++) { 152 | float Ej = (float) fabs(E[j]); 153 | m = (Em < Ej) ? j : m; 154 | Em = (Em < Ej) ? Ej : Em; 155 | } 156 | if (m != i) { 157 | int l; 158 | float b = E[i]; 159 | E[i] = E[m]; 160 | E[m] = b; 161 | for (j = 0, k = i * n, l = m * n; j < n; j++, k++, l++) { 162 | b = V[k]; 163 | V[k] = V[l]; 164 | V[l] = b; 165 | } 166 | } 167 | } 168 | 169 | return 0; 170 | } 171 | 172 | /*=========================== Double precision function ================================*/ 173 | 174 | int cvJacobiEigens_64d(double* A, double* V, double* E, int n, double eps) { 175 | int i, j, k, p, q, ind; 176 | double *A1 = A, *V1 = V, *A2 = A, *V2 = V; 177 | double Amax = 0.0, anorm = 0.0, ax, deps; 178 | 179 | /*if ( A == NULL || V == NULL || E == NULL ) return CV_NULLPTR_ERR;*/ 180 | /*if ( n <= 0 ) return CV_BADSIZE_ERR;*/ 181 | if (eps < 1.0e-15) 182 | eps = 1.0e-15; 183 | deps = eps / (double) n; 184 | 185 | /*-------- Prepare --------*/ 186 | for (i = 0; i < n; i++, V1 += n, A1 += n) { 187 | for (j = 0; j < i; j++) { 188 | double Am = A1[j]; 189 | anorm += Am * Am; 190 | } 191 | for (j = 0; j < n; j++) 192 | V1[j] = 0.0; 193 | V1[i] = 1.0; 194 | } 195 | 196 | anorm = sqrt(anorm + anorm); 197 | ax = anorm * eps / n; 198 | Amax = anorm; 199 | 200 | while (Amax > ax) { 201 | Amax /= n; 202 | do /* while (ind) */ 203 | { 204 | ind = 0; 205 | A1 = A; 206 | V1 = V; 207 | for (p = 0; p < n - 1; p++, A1 += n, V1 += n) { 208 | A2 = A + n * (p + 1); 209 | V2 = V + n * (p + 1); 210 | for (q = p + 1; q < n; q++, A2 += n, V2 += n) { 211 | double x, y, c, s, c2, s2, a; 212 | double *A3, Apq, App, Aqq, App2, Aqq2, Aip, Aiq, Vpi, Vqi; 213 | if (fabs(A1[q]) < Amax) 214 | continue; 215 | Apq = A1[q]; 216 | 217 | ind = 1; 218 | 219 | /*---- Calculation of rotation angle's sine & cosine ----*/ 220 | App = A1[p]; 221 | Aqq = A2[q]; 222 | y = 5.0e-1 * (App - Aqq); 223 | x = -Apq / sqrt(Apq * Apq + y * y); 224 | if (y < 0.0) 225 | x = -x; 226 | s = x / sqrt(2.0 * (1.0 + sqrt(1.0 - x * x))); 227 | s2 = s * s; 228 | c = sqrt(1.0 - s2); 229 | c2 = c * c; 230 | a = 2.0 * Apq * c * s; 231 | 232 | /*---- Apq annulation ----*/ 233 | A3 = A; 234 | for (i = 0; i < p; i++, A3 += n) { 235 | Aip = A3[p]; 236 | Aiq = A3[q]; 237 | Vpi = V1[i]; 238 | Vqi = V2[i]; 239 | A3[p] = Aip * c - Aiq * s; 240 | A3[q] = Aiq * c + Aip * s; 241 | V1[i] = Vpi * c - Vqi * s; 242 | V2[i] = Vqi * c + Vpi * s; 243 | } 244 | for (; i < q; i++, A3 += n) { 245 | Aip = A1[i]; 246 | Aiq = A3[q]; 247 | Vpi = V1[i]; 248 | Vqi = V2[i]; 249 | A1[i] = Aip * c - Aiq * s; 250 | A3[q] = Aiq * c + Aip * s; 251 | V1[i] = Vpi * c - Vqi * s; 252 | V2[i] = Vqi * c + Vpi * s; 253 | } 254 | for (; i < n; i++) { 255 | Aip = A1[i]; 256 | Aiq = A2[i]; 257 | Vpi = V1[i]; 258 | Vqi = V2[i]; 259 | A1[i] = Aip * c - Aiq * s; 260 | A2[i] = Aiq * c + Aip * s; 261 | V1[i] = Vpi * c - Vqi * s; 262 | V2[i] = Vqi * c + Vpi * s; 263 | } 264 | App2 = App * c2 + Aqq * s2 - a; 265 | Aqq2 = App * s2 + Aqq * c2 + a; 266 | A1[p] = App2; 267 | A2[q] = Aqq2; 268 | A1[q] = A2[p] = 0.0; 269 | } /*q*/ 270 | } /*p*/ 271 | } while (ind); 272 | } /* while ( Amax > ax ) */ 273 | 274 | for (i = 0, k = 0; i < n; i++, k += n + 1) 275 | E[i] = A[k]; 276 | 277 | /* -------- ordering --------*/ 278 | /* 279 | for (i = 0; i < n; i++) { 280 | int m = i; 281 | double Em = fabs(E[i]); 282 | for (j = i + 1; j < n; j++) { 283 | double Ej = fabs(E[j]); 284 | m = (Em < Ej) ? j : m; 285 | Em = (Em < Ej) ? Ej : Em; 286 | } 287 | if (m != i) { 288 | int l; 289 | double b = E[i]; 290 | E[i] = E[m]; 291 | E[m] = b; 292 | for (j = 0, k = i * n, l = m * n; j < n; j++, k++, l++) { 293 | b = V[k]; 294 | V[k] = V[l]; 295 | V[l] = b; 296 | } 297 | } 298 | } 299 | */ 300 | 301 | 302 | return 0; 303 | } 304 | 305 | 306 | void matrix_inverse(double T[4][4], double TIN[4][4]) { 307 | 308 | TIN[0][0] = T[0][0]; 309 | TIN[0][1] = T[1][0]; 310 | TIN[0][2] = T[2][0]; 311 | TIN[0][3] = -(T[0][0]*T[0][3] + T[1][0]*T[1][3] + T[2][0]*T[2][3]); 312 | 313 | TIN[1][0] = T[0][1]; 314 | TIN[1][1] = T[1][1]; 315 | TIN[1][2] = T[2][1]; 316 | TIN[1][3] = -(T[0][1]*T[0][3] + T[1][1]*T[1][3] + T[2][1]*T[2][3]); 317 | 318 | TIN[2][0] = T[0][2]; 319 | TIN[2][1] = T[1][2]; 320 | TIN[2][2] = T[2][2]; 321 | TIN[2][3] = -(T[0][2]*T[0][3] + T[1][2]*T[1][3] + T[2][2]*T[2][3]); 322 | 323 | TIN[3][0] = 0; 324 | TIN[3][1] = 0; 325 | TIN[3][2] = 0; 326 | TIN[3][3] = 1; 327 | } 328 | 329 | 330 | void matrix_mult(double vector[4], double matrix[4][4], double result[4]) { 331 | 332 | result[0] = matrix[0][0] * vector[0] + 333 | matrix[0][1] * vector[1] + 334 | matrix[0][2] * vector[2] + 335 | matrix[0][3] * vector[3]; 336 | 337 | result[1] = matrix[1][0] * vector[0] + 338 | matrix[1][1] * vector[1] + 339 | matrix[1][2] * vector[2] + 340 | matrix[1][3] * vector[3]; 341 | 342 | result[2] = matrix[2][0] * vector[0] + 343 | matrix[2][1] * vector[1] + 344 | matrix[2][2] * vector[2] + 345 | matrix[2][3] * vector[3]; 346 | 347 | result[3] = matrix[3][0] * vector[0] + 348 | matrix[3][1] * vector[1] + 349 | matrix[3][2] * vector[2] + 350 | matrix[3][3] * vector[3]; 351 | } 352 | 353 | 354 | /* End of file */ 355 | #endif -------------------------------------------------------------------------------- /InertiaCalculations.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 3 | * Copyright (c) 2013, Kester Duncan 4 | * 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "SQTypes.h" 13 | #include "InertiaCalculations.h" 14 | 15 | 16 | namespace ope { 17 | 18 | 19 | double InertiaCalculations::betaFunction (const double& x, const double& y) { 20 | double result = 0.0; 21 | 22 | result = (boost::math::factorial(static_cast(x - 1)) * 23 | boost::math::factorial(static_cast(y - 1))) / 24 | boost::math::factorial(static_cast(x + y - 1)); 25 | 26 | return result; 27 | } 28 | 29 | 30 | double InertiaCalculations::xMomentOfInertia(SQParameters& sq) { 31 | double result = 0.0; 32 | 33 | result = 0.5 * sq.a1 * sq.a2 * sq.a3 * sq.e1 * sq.e2 * ((sq.a2 * sq.a2) * betaFunction(1.5 * sq.e2, 0.5 * sq.e2) * 34 | betaFunction(0.5 * sq.e1, (2 * sq.e1 + 1)) + (4 * sq.a3 * sq.a3) * betaFunction(0.5 * sq.e2, (0.5 * sq.e2 + 1)) * 35 | betaFunction(1.5 * sq.e1, sq.e1 + 1)); 36 | 37 | return result; 38 | } 39 | 40 | 41 | double InertiaCalculations::yMomentOfInertia(SQParameters& sq) { 42 | double result = 0.0; 43 | 44 | result = 0.5 * sq.a1 * sq.a2 * sq.a3 * sq.e1 * sq.e2 * ((sq.a1 * sq.a1) * betaFunction(1.5 * sq.e2, 0.5 * sq.e2) * 45 | betaFunction(0.5 * sq.e1, (2 * sq.e1 + 1)) + (4 * sq.a3 * sq.a3) * betaFunction(0.5 * sq.e2, (0.5 * sq.e2 + 1)) * 46 | betaFunction(1.5 * sq.e1, sq.e1 + 1)); 47 | 48 | return result; 49 | } 50 | 51 | 52 | double InertiaCalculations::zMomentOfInertia(SQParameters& sq) { 53 | double result = 0.0; 54 | 55 | result = 0.5 * sq.a1 * sq.a2 * sq.a3 * sq.e1 * sq.e2 * ((sq.a1 * sq.a1) + (sq.a2 * sq.a2)) * 56 | betaFunction(1.5 * sq.e2, 0.5 * sq.e2) * betaFunction(0.5 * sq.e1, (2 * sq.e1 + 1)); 57 | 58 | return result; 59 | } 60 | 61 | 62 | void InertiaCalculations::calculateNewPrincipalAxis (SQParameters& sq) { 63 | double xAxisInertia = xMomentOfInertia(sq); 64 | double yAxisInertia = yMomentOfInertia(sq); 65 | double zAxisInertia = zMomentOfInertia(sq); 66 | double minInertiaValue = 99999.0; 67 | int minInertiaAxis = -1; 68 | 69 | if (xAxisInertia < minInertiaValue) { 70 | minInertiaValue = xAxisInertia; 71 | minInertiaAxis = 0; 72 | } 73 | 74 | if (yAxisInertia < minInertiaValue) { 75 | minInertiaValue = yAxisInertia; 76 | minInertiaAxis = 1; 77 | } 78 | 79 | if (zAxisInertia < minInertiaValue) { 80 | minInertiaValue = zAxisInertia; 81 | minInertiaAxis = 2; 82 | } 83 | 84 | if (minInertiaAxis == -1) { 85 | minInertiaAxis = sq.principalAxis; 86 | } 87 | 88 | sq.principalAxis = minInertiaAxis; 89 | } 90 | 91 | 92 | void InertiaCalculations::calculateMajorAxis (SQParameters& sq) { 93 | double xAxisInertia = xMomentOfInertia(sq); 94 | double yAxisInertia = yMomentOfInertia(sq); 95 | double zAxisInertia = zMomentOfInertia(sq); 96 | double minInertiaValue = 99999.0; 97 | int minInertiaAxis = -1; 98 | 99 | if (xAxisInertia < minInertiaValue) { 100 | minInertiaValue = xAxisInertia; 101 | minInertiaAxis = 0; 102 | } 103 | 104 | if (yAxisInertia < minInertiaValue) { 105 | minInertiaValue = yAxisInertia; 106 | minInertiaAxis = 1; 107 | } 108 | 109 | if (zAxisInertia < minInertiaValue) { 110 | minInertiaValue = zAxisInertia; 111 | minInertiaAxis = 2; 112 | } 113 | 114 | if (minInertiaAxis == -1) { 115 | minInertiaAxis = sq.principalAxis; 116 | } 117 | 118 | sq.majorAxis = minInertiaAxis; 119 | } 120 | 121 | 122 | void InertiaCalculations::calculateMinorAxis (SQParameters& sq) { 123 | double xAxisInertia = xMomentOfInertia(sq); 124 | double yAxisInertia = yMomentOfInertia(sq); 125 | double zAxisInertia = zMomentOfInertia(sq); 126 | double maxInertiaValue = -99999.0; 127 | int maxInertiaAxis = -1; 128 | 129 | if (xAxisInertia > maxInertiaValue) { 130 | maxInertiaValue = xAxisInertia; 131 | maxInertiaAxis = 0; 132 | } 133 | 134 | if (yAxisInertia > maxInertiaValue) { 135 | maxInertiaValue = yAxisInertia; 136 | maxInertiaAxis = 1; 137 | } 138 | 139 | if (zAxisInertia > maxInertiaValue) { 140 | maxInertiaValue = zAxisInertia; 141 | maxInertiaAxis = 2; 142 | } 143 | 144 | if (maxInertiaAxis == -1) { 145 | maxInertiaAxis = sq.principalAxis; 146 | } 147 | 148 | sq.minorAxis = maxInertiaAxis; 149 | 150 | } 151 | 152 | 153 | } /* ope namespace */ 154 | 155 | -------------------------------------------------------------------------------- /InertiaCalculations.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file InertiaCalculations.h 34 | * \brief Functions to calculate the moments of inertia of superquadrics. 35 | * This is used to calculate the principal axis of a superquadric, which 36 | * coincides with the axis of least inertia 37 | * \author Kester Duncan 38 | */ 39 | #pragma once 40 | #ifndef _INERTIA_CALCULATIONS_H_ 41 | #define _INERTIA_CALCULATIONS_H_ 42 | 43 | 44 | namespace ope { 45 | 46 | 47 | // Forward declaration 48 | class SQParameters; 49 | 50 | 51 | struct InertiaCalculations { 52 | /** 53 | * \brief Beta function (or Euler integral of the first kind) 54 | * For more information, visit: http://en.wikipedia.org/wiki/Beta_function 55 | */ 56 | double betaFunction (const double& x, const double& y); 57 | 58 | 59 | /** 60 | * \brief Calculates the moment of inertia along the x axis 61 | */ 62 | double xMomentOfInertia(SQParameters& sq); 63 | 64 | 65 | /** 66 | * \brief Calculates the moment of inertia along the y axis 67 | */ 68 | double yMomentOfInertia(SQParameters& sq); 69 | 70 | 71 | /** 72 | * \brief Calculates the moment of inertia along the z axis 73 | */ 74 | double zMomentOfInertia(SQParameters& sq); 75 | 76 | 77 | /** 78 | * \brief Calculates the principal axis of a superquadric 79 | */ 80 | void calculateNewPrincipalAxis (SQParameters& sq); 81 | 82 | 83 | /** 84 | * \brief Calculates the major axis of a superquadric 85 | */ 86 | void calculateMajorAxis (SQParameters& sq); 87 | 88 | 89 | /** 90 | * \brief Calculates the minor axis of a superquadric 91 | */ 92 | void calculateMinorAxis (SQParameters& sq); 93 | 94 | 95 | }; 96 | 97 | 98 | } /* ope namespace */ 99 | 100 | #endif /*_INERTIA_CALCULATIONS_H_ */ -------------------------------------------------------------------------------- /Minimization.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * Adapted from code by G. Biegelbauer 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "SQTypes.h" 17 | #include "Minimization.h" 18 | 19 | 20 | namespace ope { 21 | 22 | Minimization::Minimization() { 23 | glochisq = 0.; 24 | i_am_in_trouble = 0; 25 | 26 | for (int i = 0; i < MAX_PARAMS; i++) { 27 | glatry[i] = 0; 28 | glbeta[i] = 0; 29 | } 30 | 31 | } 32 | 33 | 34 | Minimization::~Minimization() { 35 | 36 | } 37 | 38 | 39 | 40 | double Minimization::mylog(double x) { 41 | if (x > 0) { 42 | return(log(x)); 43 | } else { 44 | return(0); 45 | } 46 | } 47 | 48 | 49 | double Minimization::mypow(double x, double y) { 50 | if (x >= 0) { 51 | return(pow(x,y)); 52 | } else { 53 | return(pow(fabs(x),y)); 54 | } 55 | } 56 | 57 | 58 | double Minimization::sqr(double x) { 59 | return (x * x); 60 | } 61 | 62 | 63 | double Minimization::errorFunc(const pcl::PointCloud& cloud, SQParameters& sqParams) { 64 | pcl::PointXYZ _pnt; 65 | Eigen::MatrixXd m(3, 3), _m(3, 3); 66 | 67 | m(0, 0) = cos(sqParams.phi) * cos(sqParams.theta) * cos(sqParams.psi) - sin(sqParams.phi) * sin(sqParams.psi); 68 | m(0, 1) = -cos(sqParams.phi) * cos(sqParams.theta) * sin(sqParams.psi) - sin(sqParams.phi) * cos(sqParams.psi); 69 | m(0, 2) = cos(sqParams.phi) * sin(sqParams.theta); 70 | m(1, 0) = sin(sqParams.phi) * cos(sqParams.theta) * cos(sqParams.psi) + cos(sqParams.phi) * sin(sqParams.psi); 71 | m(1, 1) = -sin(sqParams.phi) * cos(sqParams.theta) * sin(sqParams.psi) + cos(sqParams.phi) * cos(sqParams.psi); 72 | m(1, 2) = sin(sqParams.phi) * sin(sqParams.theta); 73 | m(2, 0) = -sin(sqParams.theta) * cos(sqParams.psi); 74 | m(2, 1) = sin(sqParams.theta) * sin(sqParams.psi); 75 | m(2, 2) = cos(sqParams.theta); 76 | 77 | _m = m.inverse(); 78 | 79 | double f, a, b, c, d, e, error = 0; 80 | 81 | for (int i = 0; i < (int) cloud.points.size(); i++) { 82 | Eigen::MatrixXd pntMove(3, 1); 83 | Eigen::MatrixXd pnt(3, 1); 84 | Eigen::MatrixXd _pnt(3, 1); 85 | 86 | pntMove(0, 0) = (double) cloud.points[i].x - sqParams.px; 87 | pntMove(1, 0) = (double) cloud.points[i].y - sqParams.py; 88 | pntMove(2, 0) = (double) cloud.points[i].z - sqParams.pz; 89 | 90 | _pnt = _m * pntMove; 91 | 92 | _pnt(0, 0) = _pnt(0, 0) / (sqParams.kx * _pnt(2, 0) / sqParams.a3 + 1); 93 | _pnt(1, 0) = _pnt(1, 0) / (sqParams.ky * _pnt(2, 0) / sqParams.a3 + 1); 94 | 95 | a = pow(fabs(_pnt(0, 0) / sqParams.a1), 2.0 / sqParams.e2); 96 | b = pow(fabs(_pnt(1, 0) / sqParams.a2), 2.0 / sqParams.e2); 97 | c = pow(fabs(_pnt(2, 0) / sqParams.a3), 2.0 / sqParams.e1); 98 | d = pow(a + b, (double) (sqParams.e2 / sqParams.e1)); 99 | e = d + c; 100 | f = pow(e, (double) (sqParams.e1)); 101 | 102 | double tempError = pow((f - 1), 2.0); 103 | error += exp(-tempError * 10000); 104 | } 105 | 106 | error = error / static_cast(cloud.points.size()); 107 | error = sqrt(error); 108 | 109 | return (error); 110 | } 111 | 112 | 113 | double Minimization::funcs(double x, double y, double z, 114 | double rotx[11], double roty[11], double rotz[11], 115 | glnparam a, glnparam dFda) { 116 | 117 | //double coefx, coefy, coefz, A, B, D, G, F, DA, DB, GD, Gz, R, M, N, nx, ny, nz, nz_p, N1, N2, SAB, CAB; 118 | //glnparam dx, dy, dz, dxb, dyb, dzb, dA, dB, dD, dG, dR; 119 | //double xb, yb, zb, koren; 120 | //double O, Omin, Omax, zhat; 121 | 122 | // Eliminated unreferenced local variables 123 | double coefx, coefy, coefz, A, B, D, G, F, DA, DB, GD, Gz, R, SAB, CAB; 124 | glnparam dx, dy, dz, dxb, dyb, dzb, dA, dB, dD, dG, dR; 125 | double xb, yb, zb, koren; 126 | double O; 127 | 128 | coefx = (rotx[0]*x + roty[0]*y + rotz[0]*z - (a[8]*rotx[0] + a[9]*roty[0] + a[10]*rotz[0])); 129 | 130 | for (int i = 0; i < 5; i++) dx[i] = 0; 131 | 132 | dx[5] = (rotx[3] * x + roty[3] * y + rotz[3] * z - (a[8]*rotx[3] + a[9]*roty[3] + a[10]*rotz[3])); 133 | dx[6] = (rotx[6] * x + roty[6] * y + rotz[6] * z - (a[8]*rotx[6] + a[9]*roty[6] + a[10]*rotz[6])); 134 | dx[7] = (rotx[9] * x + roty[9] * y + rotz[9] * z - (a[8]*rotx[9] + a[9]*roty[9] + a[10]*rotz[9])); 135 | 136 | dx[8] = -rotx[0]; 137 | dx[9] = -roty[0]; 138 | dx[10] = -rotz[0]; 139 | 140 | for (int i = 11; i < 18; i++) dx[i] = 0; 141 | 142 | coefy = (rotx[1]*x + roty[1]*y + rotz[1]*z - (a[8]*rotx[1] + a[9]*roty[1] + a[10]*rotz[1])); 143 | 144 | for (int i = 0; i < 5; i++) dy[i] = 0; 145 | 146 | dy[5] = (rotx[4] * x + roty[4] * y + rotz[4] * z - (a[8]*rotx[4] + a[9]*roty[4] + a[10]*rotz[4])); 147 | dy[6] = (rotx[7] * x + roty[7] * y + rotz[7] * z - (a[8]*rotx[7] + a[9]*roty[7] + a[10]*rotz[7])); 148 | dy[7] = (rotx[10] * x + roty[10] * y + rotz[10] * z - (a[8]*rotx[10] + a[9]*roty[10] + a[10]*rotz[10])); 149 | dy[8] = -rotx[1]; 150 | dy[9] = -roty[1]; 151 | dy[10] = -rotz[1]; 152 | 153 | for (int i = 11; i < 18; i++) dy[i] = 0; 154 | 155 | coefz = (rotx[2]*x + roty[2]*y + rotz[2]*z - (a[8]*rotx[2] + a[9]*roty[2] + a[10]*rotz[2])); 156 | 157 | for (int i = 0; i < 5; i++) dz[i] = 0; 158 | 159 | dz[5] = (rotx[5] * x + roty[5] * y + rotz[5] * z - (a[8]*rotx[5] + a[9]*roty[5] + a[10]*rotz[5])); 160 | dz[6] = (rotx[8] * x + roty[8] * y + rotz[8] * z - (a[8]*rotx[8] + a[9]*roty[8] + a[10]*rotz[8])); 161 | dz[7] = 0; 162 | dz[8] = -rotx[2]; 163 | dz[9] = -roty[2]; 164 | dz[10] = -rotz[2]; 165 | 166 | for (int i = 11; i < 18; i++) dz[i] = 0; 167 | 168 | koren = sqrt(sqr(coefx) + sqr(coefy)); 169 | SAB = sin(a[17] - atan2(coefy, coefx)); 170 | CAB = cos(a[17] - atan2(coefy, coefx)); 171 | 172 | R = 1/(koren * CAB); 173 | 174 | for (int i = 0; i < 5; i++) 175 | dR[i] = 0; 176 | 177 | for (int i = 5; i < 11; i++) 178 | dR[i] = (-(coefx * dx[i] + coefy * dy[i]) / CAB - (SAB/sqr(CAB)) * (coefx * dy[i] - coefy * dx[i])) / (koren * sqr(koren)); 179 | 180 | for (int i = 11; i < 16; i++) 181 | dR[i] = 0; 182 | 183 | dR[17] = SAB/(koren * sqr(CAB)); 184 | 185 | 186 | /**********************************************/ 187 | 188 | koren = sqrt(sqr(coefz) + sqr(1/a[16] - 1/R)); 189 | 190 | xb = coefx - (1/R - 1/a[16] + koren) * cos(a[17]); 191 | 192 | for (int i = 0; i < 5; i++) 193 | dxb[i] = 0; 194 | 195 | for (int i = 0; i < 11; i++) 196 | dxb[i] = dx[i] - (-dR[i]/sqr(R) + (1.0/koren) * (coefz * dz[i] + (1/a[16] - 1/R) * (dR[i]/sqr(R)))) * cos(a[17]); 197 | 198 | for (int i = 11; i < 16; i++) 199 | dxb[i] = 0; 200 | 201 | dxb[16] = cos(a[17]) * (-1/sqr(a[16]) + (1/a[16] - 1/R)/(koren * sqr(a[16]))); 202 | dxb[17] = sin(a[17]) * (1/R - 1/a[16] + koren) + cos(a[17]) * dR[17] * (1/sqr(R) - (1/a[16] - 1/R)/(koren*sqr(R))); 203 | 204 | 205 | /************************************************/ 206 | 207 | yb = coefy - (1/R - 1/a[16] + koren) * sin(a[17]); 208 | 209 | for (int i = 0; i < 5; i++) 210 | dyb[i] = 0; 211 | 212 | for (int i = 0; i < 11; i++) 213 | dyb[i] = dy[i] - (-dR[i]/sqr(R) + (1.0/koren) * (coefz * dz[i] + (1/a[16] - 1/R) * dR[i]/sqr(R))) * sin(a[17]); 214 | 215 | for (int i = 11; i < 16; i++) 216 | dyb[i] = 0; 217 | 218 | dyb[16] = sin(a[17]) * (-1/sqr(a[16]) + (1/a[16] - 1/R) / (koren * sqr(a[16]))); 219 | dyb[17] = -cos(a[17]) * (1/R - 1/a[16] + koren) - sin(a[17]) * dR[17] * (-1/sqr(R) + (1/a[16] - 1/R)/(koren* sqr(R))); 220 | 221 | 222 | /***************************************************************/ 223 | 224 | O = atan2(coefz, 1/a[16] - 1/R); 225 | 226 | zb = O/a[16]; 227 | 228 | for (int i = 0; i < 5; i++) 229 | dzb[i] = 0; 230 | 231 | for (int i = 5; i < 11; i++) 232 | dzb[i] = (1/(a[16]*(sqr(1/a[16] - 1/R) + sqr(coefz)))) * ((1/a[16] - 1/R) * dz[i] - coefz * dR[i]/sqr(R)); 233 | 234 | for (int i = 11; i < 16; i++) 235 | dzb[i] = 0; 236 | 237 | dzb[16] = -O/sqr(a[16]) + (1/(a[16] * sqr(a[16]))) * coefz/(sqr(coefz) + sqr(1/a[16] - 1/R)); 238 | dzb[17] = 0; 239 | 240 | /******************************************************************/ 241 | 242 | A = xb/(a[0]*(a[11]*zb/a[2] + 1)); 243 | 244 | dA[0] = -A/a[0]; 245 | dA[1] = 0; 246 | dA[2] = (xb/a[0]) * (1.0/sqr(a[11]*zb/a[2] + 1))*a[11]*zb/sqr(a[2]); 247 | 248 | for(int i = 3; i < 11; i++) 249 | dA[i] = dxb[i]/(a[0]*(a[11]*zb/a[2] + 1)) - xb * a[11] * dzb[i]/(a[0]*sqr(a[11] * zb/a[2] + 1)*a[2]); 250 | 251 | dA[11] = -xb * zb/(a[0]*sqr(a[11]*zb/a[2] + 1)*a[2]); 252 | dA[12] = 0; 253 | 254 | for(int i = 13; i < 18; i++) 255 | dA[i] = dxb[i]/(a[0]*(a[11]*zb/a[2] + 1)) - xb * a[11] * dzb[i]/(a[0]*sqr(a[11] * zb/a[2] + 1)*a[2]); 256 | 257 | B = yb/(a[1]*(a[12]*zb/a[2] + 1)); 258 | 259 | dB[0] = 0; 260 | dB[1] = -B/a[1]; 261 | dB[2] = (yb/a[1]) * (1.0/sqr(a[12]*zb/a[2] + 1))*a[12]*zb/sqr(a[2]); 262 | 263 | for(int i = 3; i < 11; i++) 264 | dB[i] = dyb[i]/(a[1]*(a[12]*zb/a[2] + 1)) - yb * a[12] * dzb[i]/(a[1]*sqr(a[12] * zb/a[2] + 1)*a[2]); 265 | 266 | dB[11] = 0; 267 | dB[12] = -yb * zb/(a[1]*sqr(a[12]*zb/a[2] + 1)*a[2]); 268 | 269 | for(int i = 13; i < 18; i++) 270 | dB[i] = dyb[i]/(a[1]*(a[12]*zb/a[2] + 1)) - yb * a[12] * dzb[i]/(a[1]*sqr(a[12] * zb/a[2] + 1)*a[2]); 271 | 272 | 273 | /*********************************************/ 274 | DA = mypow(sqr(A), 1.0/a[4]); DB = mypow(sqr(B), 1.0/a[4]); 275 | D = DA + DB; 276 | 277 | for(int i = 0; i < 3; i++) 278 | dD[i] = (2/a[4]) * (DA/A) * dA[i] + (2/a[4]) * (DB/B) * dB[i]; 279 | 280 | dD[3] = 0; 281 | dD[4] = -DA * mylog(sqr(A))/sqr(a[4]) - DB * mylog(sqr(B))/sqr(a[4]); 282 | 283 | for(int i = 5; i < 18; i++) 284 | dD[i] = (2/a[4]) * (DA/A) * dA[i] + (2/a[4]) * (DB/B) * dB[i]; 285 | 286 | /**********************************************/ 287 | GD = mypow(D, a[4]/a[3]); Gz = mypow(sqr(zb/a[2]), 1.0/a[3]); 288 | G = GD + Gz; 289 | 290 | for(int i = 0; i < 2; i++) 291 | dG[i] = (a[4]/a[3]) * (GD/D) * dD[i]; 292 | 293 | dG[2] = (a[4]/a[3]) * (GD/D) * dD[2] - (2/a[3]) * (a[2]/zb)* Gz * (zb/sqr(a[2])); 294 | dG[3] = GD * (-a[4] * (1/sqr(a[3])) * mylog(D) + (a[4]/a[3])*(1/D) * dD[3]) - (1.0/sqr(a[3])) * Gz * mylog(sqr(zb/a[2])); 295 | dG[4] = GD * ((1/a[3]) * mylog(D) + (a[4]/a[3])*(1/D) * dD[4]); 296 | 297 | for(int i = 5; i < 18; i++) 298 | dG[i] = (a[4]/a[3]) * (GD/D) * dD[i] + (2/a[3]) * (Gz/zb) * dzb[i]; 299 | 300 | /***********************************************/ 301 | F = mypow(G, a[3]); 302 | for(int i= 0; i < 3; i++) 303 | dFda[i] = a[3] * (F/G) * dG[i]; 304 | 305 | dFda[3] = F * (mylog(G) + (a[3]/G) * dG[3]); 306 | 307 | for(int i = 4; i < 18; i++) 308 | dFda[i] = a[3] * (F/G) * dG[i]; 309 | 310 | F = F - 1; 311 | 312 | dFda[0] = sqrt(a[1] * a[2]) * (F/(2*sqrt(a[0])) + sqrt(a[0]) * dFda[0]); 313 | dFda[1] = sqrt(a[0] * a[2]) * (F/(2*sqrt(a[1])) + sqrt(a[1]) * dFda[1]); 314 | dFda[2] = sqrt(a[0] * a[1]) * (F/(2*sqrt(a[2])) + sqrt(a[2]) * dFda[2]); 315 | 316 | for(int i = 3; i < 18; i++) 317 | dFda[i] = dFda[i] * sqrt(a[0] * a[1] * a[2]); 318 | 319 | F = sqrt(a[0] * a[1] * a[2]) * F; 320 | 321 | if(F < 0) { 322 | return(1 * F); 323 | } else { 324 | return(F); 325 | } 326 | } 327 | 328 | 329 | void Minimization::precomp_rot(glnparam a, double rotx[11], double roty[11], double rotz[11]) { 330 | rotx[0] = (cos(a[5])*cos(a[6])*cos(a[7]) - sin(a[5])*sin(a[7])); 331 | rotx[1] = (-cos(a[5])*cos(a[6])*sin(a[7]) - sin(a[5])*cos(a[7])); 332 | rotx[2] = (cos(a[5])*sin(a[6])); 333 | 334 | rotx[3] = (-sin(a[5])*cos(a[6])*cos(a[7]) - cos(a[5])*sin(a[7])); 335 | rotx[4] = (sin(a[5])*cos(a[6])*sin(a[7]) - cos(a[5])*cos(a[7])); 336 | rotx[5] = (-sin(a[5])*sin(a[6])); 337 | 338 | rotx[6] = (-cos(a[5])*sin(a[6])*cos(a[7])); 339 | rotx[7] = (cos(a[5])*sin(a[6])*sin(a[7])); 340 | rotx[8] = (cos(a[5])*cos(a[6])); 341 | 342 | rotx[9] = (-cos(a[5])*cos(a[6])*sin(a[7]) - sin(a[5])*cos(a[7])); 343 | rotx[10]= (-cos(a[5])*cos(a[6])*cos(a[7]) + sin(a[5])*sin(a[7])); 344 | 345 | roty[0] = (sin(a[5])*cos(a[6])*cos(a[7]) + cos(a[5])*sin(a[7])); 346 | roty[1] = (-sin(a[5])*cos(a[6])*sin(a[7]) + cos(a[5])*cos(a[7])); 347 | roty[2] = (sin(a[5])*sin(a[6])); 348 | 349 | roty[3] = (cos(a[5])*cos(a[6])*cos(a[7]) - sin(a[5])*sin(a[7])); 350 | roty[4] = (-cos(a[5])*cos(a[6])*sin(a[7]) - sin(a[5])*cos(a[7])); 351 | roty[5] = (cos(a[5])*sin(a[6])); 352 | 353 | roty[6] = (-sin(a[5])*sin(a[6])*cos(a[7])); 354 | roty[7] = (sin(a[5])*sin(a[6])*sin(a[7])); 355 | roty[8] = (sin(a[5])*cos(a[6])); 356 | 357 | roty[9] = (-sin(a[5])*cos(a[6])*sin(a[7]) + cos(a[5])*cos(a[7])); 358 | roty[10]= (-sin(a[5])*cos(a[6])*cos(a[7]) - cos(a[5])*sin(a[7])); 359 | 360 | rotz[0] = (-sin(a[6])*cos(a[7])); 361 | rotz[1] = (sin(a[6])*sin(a[7])); 362 | rotz[2] = (cos(a[6])); 363 | 364 | rotz[3] = 0; 365 | rotz[4] = 0; 366 | rotz[5] = 0; 367 | 368 | rotz[6] = (-cos(a[6])*cos(a[7])); 369 | rotz[7] = (cos(a[6])*sin(a[7])); 370 | rotz[8] = (-sin(a[6])); 371 | 372 | rotz[9] = (sin(a[6])*sin(a[7])); 373 | rotz[10] = (sin(a[6])*cos(a[7])); 374 | 375 | } 376 | 377 | 378 | double Minimization::mrqcof(const glndata2& x, glndata F, glndata sig, 379 | int ndata, glmma a, gllista lista, int mfit, 380 | glcovar alpha, glmma beta, int *n_model, 381 | int *n_model_acc, double addnoise) { 382 | 383 | double Fmod, wt, sig2i, dF, chisq, threshold; 384 | glmma dFda; 385 | double rotx[11], roty[11], rotz[11]; 386 | 387 | for(int j = 0; j < mfit; j++) { 388 | for(int k = 0; k <= j; k++) { 389 | alpha[j][k] = 0.0; 390 | } 391 | beta[j] = 0.0; 392 | } 393 | 394 | sig2i = 1.0 / (sig[1] * sig[1]); 395 | precomp_rot(a, rotx, roty, rotz); 396 | threshold = 10e30; // 1.068647458e14 397 | 398 | do { 399 | chisq = 0.0; 400 | *n_model = 0; 401 | for(int i = 0; i < ndata; i++) { 402 | Fmod = funcs(x[i][0], x[i][1], x[i][2], rotx, roty, rotz, a, dFda); 403 | dF = F[i] - Fmod; 404 | 405 | if (Fmod < threshold) { 406 | (*n_model) += 1; 407 | 408 | for(int j = 0; j < mfit; j++) { 409 | wt = dFda[lista[j]]*sig2i; 410 | 411 | for(int k = 0; k <= j; k++) { 412 | alpha[j][k] = alpha[j][k] + wt * dFda[lista[k]]; 413 | } 414 | beta[j] = beta[j] + dF*wt; 415 | } 416 | chisq = chisq + dF*dF*sig2i; 417 | } 418 | 419 | /***** The iteration cannot be accepted *********/ 420 | 421 | if(chisq / (*n_model_acc) > addnoise) break; 422 | } 423 | threshold = threshold * 2; 424 | 425 | if(chisq / (*n_model_acc) > addnoise) break; 426 | 427 | } while ((double)(*n_model) / (*n_model_acc) < 0.95); 428 | 429 | for(int j = 0; j < mfit; j++) { 430 | for(int k = 0; k <= j ; k++) { 431 | alpha[j][k] = alpha[j][k] * chisq; 432 | } 433 | beta[j] = beta[j] * chisq; 434 | } 435 | 436 | chisq = chisq / (*n_model); 437 | 438 | for(int j = 1; j < mfit; j++) { 439 | for(int k = 0; k <= j-1; k++) { 440 | alpha[k][j] = alpha[j][k]; 441 | } 442 | } 443 | 444 | return(chisq); 445 | } 446 | 447 | 448 | int Minimization::gaussj(glcovar a, int n, glcovar b) { 449 | double big, dum, pivinv; 450 | int icol, irow, m; 451 | 452 | glnp indxc, indxr, ipiv; 453 | 454 | m = 1; 455 | 456 | for(int j = 0; j < n; j++) ipiv[j] = 0; 457 | 458 | for(int i = 0; i < n; i++) { 459 | big = 0.0; 460 | for(int j = 0; j < n; j++) { 461 | if (ipiv[j] != 1) { 462 | for(int k = 0; k < n ; k++) { 463 | if(ipiv[k] == 0) { 464 | if (fabs(a[j][k]) >= big) { 465 | big = fabs(a[j][k]); 466 | irow = j; 467 | icol = k; 468 | } 469 | } 470 | else if (ipiv[k] > 1) { 471 | //printf("pause 1 in GAUSSJ - singular matrix\n"); 472 | return(0); 473 | } 474 | } 475 | } 476 | } 477 | 478 | ipiv[icol] = ipiv[icol] + 1; 479 | if (irow != icol) { 480 | for(int l = 0; l < n; l++) { 481 | dum = a[irow][l]; 482 | a[irow][l] = a[icol][l]; 483 | a[icol][l] = dum; 484 | } 485 | for(int l = 0; l < m; l++) { 486 | dum = b[irow][l]; 487 | b[irow][l] = b[icol][l]; 488 | b[icol][l] = dum; 489 | } 490 | } 491 | 492 | indxr[i] = irow; 493 | indxc[i] = icol; 494 | //if (a[icol][icol] == 0.0) printf("pause 2 in GAUSSJ - singular matrix\n"); 495 | 496 | pivinv = 1.0/a[icol][icol]; 497 | a[icol][icol] = 1.0; 498 | for(int l = 0; l < n; l++) a[icol][l] = a[icol][l] *pivinv; 499 | for(int l = 0; l < m; l ++) b[icol][l] = b[icol][l] * pivinv; 500 | for(int ll = 0; ll < n; ll++) { 501 | if (ll != icol) { 502 | dum = a[ll][icol]; 503 | a[ll][icol] = 0.0; 504 | for(int l = 0; l < n; l++) a[ll][l] = a[ll][l] - a[icol][l]* dum; 505 | for(int l = 0; l < m; l++) b[ll][l] = b[ll][l] - b[icol][l]*dum; 506 | } 507 | } 508 | } 509 | 510 | for(int l = n-1; l >= 0; l--) { 511 | if (indxr[l] != indxc[l]) { 512 | for(int k = 0; k < n; k++) { 513 | dum = a[k][indxr[l]]; 514 | a[k][indxr[l]] = a[k][indxc[l]]; 515 | a[k][indxc[l]] = dum; 516 | } 517 | } 518 | } 519 | 520 | return(1); 521 | } 522 | 523 | 524 | double Minimization::mrqmin_init(const glndata2& x, glndata F, glndata sig, int ndata, 525 | glmma a, gllista lista, int mfit, glcovar alpha, int nca, int *n_model_acc) { 526 | 527 | int kk, ihit, n_model; 528 | double alamda, addnoise; 529 | 530 | kk = mfit; 531 | for(int j = 0; j < nca; j++) { 532 | ihit = 0; 533 | for(int k = 0; k < mfit; k++) { 534 | if (lista[k] == j) ihit = ihit + 1; 535 | } 536 | 537 | if (ihit == 0) { 538 | lista[kk] = j; 539 | kk = kk + 1; 540 | } 541 | else if (ihit > 1) { 542 | printf("WARNING: pause 1 in routine MRQMIN\n"); 543 | printf("WARNING: Improper permutation in LISTA\n"); 544 | } 545 | } 546 | 547 | if(kk != nca) { 548 | printf("WARNING: pause 2 in routine MRQMIN\n"); 549 | printf("WARNING:Improper permutation in LISTA\n"); 550 | } 551 | 552 | alamda = 1; 553 | addnoise = 1e20; // 485165195.4 554 | glochisq = mrqcof(x, F, sig, ndata, a, lista, mfit, alpha, glbeta, &n_model, n_model_acc, addnoise); 555 | *n_model_acc = n_model; 556 | 557 | return(alamda); 558 | } 559 | 560 | 561 | double Minimization::mrqmin(SQParameters& prm, const glndata2& x, glndata F, glndata sig, 562 | int ndata, glmma a, gllista lista, int mfit, glcovar covar, glcovar alpha, 563 | double alamda, int *n_model_acc) { 564 | 565 | srand(static_cast(time(NULL))); 566 | double chisq; 567 | double poisson, addnoise; 568 | glmma da; 569 | glcovar oneda; 570 | int n_model; 571 | 572 | for(int j = 0; j < mfit; j++) { 573 | for(int k = 0; k < mfit; k++) covar[j][k] = alpha[j][k]; 574 | covar[j][j] = alpha[j][j] * (0.1 + alamda); 575 | oneda[j][0] = glbeta[j]; 576 | } 577 | 578 | if (gaussj(covar, mfit, oneda) == 0) { 579 | i_am_in_trouble = 1; 580 | return(0); 581 | } 582 | 583 | for(int j = 0; j < mfit; j++) { 584 | da[j] = oneda[j][0]; 585 | } 586 | 587 | for (int j = 0; j < mfit; j++) { 588 | if (lista[j] == 0) { 589 | if (prm.min.a1.type == BOUNDED) { 590 | if(a[lista[j]] + da[j] <= prm.min.a1.lowerBound) { 591 | glatry[lista[j]] = prm.min.a1.lowerBound; 592 | } else if(a[lista[j]] + da[j] >= prm.min.a1.upperBound) { 593 | glatry[lista[j]] = prm.min.a1.upperBound; 594 | } else { 595 | glatry[lista[j]] = a[lista[j]] + da[j]; 596 | } 597 | } 598 | else { 599 | if (a[lista[j]] + da[j] < 1.0) glatry[lista[j]] = 1.0; 600 | else glatry[lista[j]] = a[lista[j]] + da[j]; 601 | } 602 | } 603 | 604 | else if (lista[j] == 1) { 605 | if (prm.min.a2.type == BOUNDED) { 606 | if(a[lista[j]] + da[j] <= prm.min.a2.lowerBound) glatry[lista[j]] = prm.min.a2.lowerBound; 607 | else if(a[lista[j]] + da[j] >= prm.min.a2.upperBound) glatry[lista[j]] = prm.min.a2.upperBound; 608 | else glatry[lista[j]] = a[lista[j]] + da[j]; 609 | } 610 | else { 611 | if (a[lista[j]] + da[j] < 1.0) glatry[lista[j]] = 1.0; 612 | else glatry[lista[j]] = a[lista[j]] + da[j]; 613 | } 614 | } 615 | 616 | else if (lista[j] == 2) { 617 | if (prm.min.a3.type == BOUNDED) { 618 | if(a[lista[j]] + da[j] <= prm.min.a3.lowerBound) glatry[lista[j]] = prm.min.a3.lowerBound; 619 | else if(a[lista[j]] + da[j] >= prm.min.a3.upperBound) glatry[lista[j]] = prm.min.a3.upperBound; 620 | else glatry[lista[j]] = a[lista[j]] + da[j]; 621 | } 622 | else { 623 | if (a[lista[j]] + da[j] < 1.0) glatry[lista[j]] = 1.0; 624 | else glatry[lista[j]] = a[lista[j]] + da[j]; 625 | } 626 | } 627 | 628 | else if (lista[j] == 3) { 629 | if (prm.min.e1.type == BOUNDED) { 630 | if(a[lista[j]] + da[j] <= prm.min.e1.lowerBound) glatry[lista[j]] = prm.min.e1.lowerBound; 631 | else if(a[lista[j]] + da[j] >= prm.min.e1.upperBound) glatry[lista[j]] = prm.min.e1.upperBound; 632 | else glatry[lista[j]] = a[lista[j]] + da[j]; 633 | } 634 | else { 635 | if(a[lista[j]] + da[j] < 0.1) glatry[lista[j]] = 0.1; 636 | else if(a[lista[j]] + da[j] > 1.0) glatry[lista[j]] = 1.0; 637 | else glatry[lista[j]] = a[lista[j]] + da[j]; 638 | } 639 | } 640 | 641 | else if (lista[j] == 4) { 642 | if (prm.min.e2.type == BOUNDED) { 643 | if(a[lista[j]] + da[j] <= prm.min.e2.lowerBound) glatry[lista[j]] = prm.min.e2.lowerBound; 644 | else if(a[lista[j]] + da[j] >= prm.min.e2.upperBound) glatry[lista[j]] = prm.min.e2.upperBound; 645 | else glatry[lista[j]] = a[lista[j]] + da[j]; 646 | } 647 | else { 648 | if(a[lista[j]] + da[j] < 0.1) glatry[lista[j]] = 0.1; 649 | else if(a[lista[j]] + da[j] > 1.0) glatry[lista[j]] = 1.0; 650 | else glatry[lista[j]] = a[lista[j]] + da[j]; 651 | } 652 | } 653 | 654 | else if (lista[j] == 5) { 655 | if (prm.min.phi.type == BOUNDED) { 656 | if(a[lista[j]] + da[j] <= prm.min.phi.lowerBound) glatry[lista[j]] = prm.min.phi.lowerBound; 657 | else if(a[lista[j]] + da[j] >= prm.min.phi.upperBound) glatry[lista[j]] = prm.min.phi.upperBound; 658 | else glatry[lista[j]] = a[lista[j]] + da[j]; 659 | } 660 | else glatry[lista[j]] = a[lista[j]] + da[j]; 661 | } 662 | 663 | else if (lista[j] == 6) { 664 | if (prm.min.theta.type == BOUNDED) { 665 | if(a[lista[j]] + da[j] <= prm.min.theta.lowerBound) glatry[lista[j]] = prm.min.theta.lowerBound; 666 | else if(a[lista[j]] + da[j] >= prm.min.phi.upperBound) glatry[lista[j]] = prm.min.theta.upperBound; 667 | else glatry[lista[j]] = a[lista[j]] + da[j]; 668 | } 669 | else glatry[lista[j]] = a[lista[j]] + da[j]; 670 | } 671 | 672 | else if (lista[j] == 7) { 673 | if (prm.min.psi.type == BOUNDED) { 674 | if(a[lista[j]] + da[j] <= prm.min.psi.lowerBound) glatry[lista[j]] = prm.min.psi.lowerBound; 675 | else if(a[lista[j]] + da[j] >= prm.min.psi.upperBound) glatry[lista[j]] = prm.min.psi.upperBound; 676 | else glatry[lista[j]] = a[lista[j]] + da[j]; 677 | } 678 | else glatry[lista[j]] = a[lista[j]] + da[j]; 679 | } 680 | 681 | else if (lista[j] == 8) { 682 | if (prm.min.px.type == BOUNDED) { 683 | if(a[lista[j]] + da[j] <= prm.min.px.lowerBound) glatry[lista[j]] = prm.min.px.lowerBound; 684 | else if(a[lista[j]] + da[j] >= prm.min.px.upperBound) glatry[lista[j]] = prm.min.px.upperBound; 685 | else glatry[lista[j]] = a[lista[j]] + da[j]; 686 | } 687 | else glatry[lista[j]] = a[lista[j]] + da[j]; 688 | } 689 | 690 | else if (lista[j] == 9) { 691 | if (prm.min.py.type == BOUNDED) { 692 | if(a[lista[j]] + da[j] <= prm.min.py.lowerBound) glatry[lista[j]] = prm.min.py.lowerBound; 693 | else if(a[lista[j]] + da[j] >= prm.min.py.upperBound) glatry[lista[j]] = prm.min.py.upperBound; 694 | else glatry[lista[j]] = a[lista[j]] + da[j]; 695 | } 696 | else glatry[lista[j]] = a[lista[j]] + da[j]; 697 | } 698 | 699 | else if (lista[j] == 10) { 700 | if (prm.min.pz.type == BOUNDED) { 701 | if(a[lista[j]] + da[j] <= prm.min.pz.lowerBound) glatry[lista[j]] = prm.min.pz.lowerBound; 702 | else if(a[lista[j]] + da[j] >= prm.min.pz.upperBound) glatry[lista[j]] = prm.min.pz.upperBound; 703 | else glatry[lista[j]] = a[lista[j]] + da[j]; 704 | } 705 | else glatry[lista[j]] = a[lista[j]] + da[j]; 706 | } 707 | 708 | else if (lista[j] == 11) { 709 | if (prm.min.kx.type == BOUNDED) { 710 | if(a[lista[j]] + da[j] <= prm.min.kx.lowerBound) glatry[lista[j]] = prm.min.kx.lowerBound; 711 | else if(a[lista[j]] + da[j] >= prm.min.kx.upperBound) glatry[lista[j]] = prm.min.kx.upperBound; 712 | else glatry[lista[j]] = a[lista[j]] + da[j]; 713 | } 714 | else { 715 | if(a[lista[j]] + da[j] > 1) glatry[lista[j]] = 1; 716 | else if(a[lista[j]] + da[j] < -1) glatry[lista[j]] = -1; 717 | else glatry[lista[j]] = a[lista[j]] + da[j]; 718 | } 719 | } 720 | 721 | else if (lista[j] == 12) { 722 | if (prm.min.ky.type == BOUNDED) { 723 | if(a[lista[j]] + da[j] <= prm.min.ky.lowerBound) glatry[lista[j]] = prm.min.ky.lowerBound; 724 | else if(a[lista[j]] + da[j] >= prm.min.ky.upperBound) glatry[lista[j]] = prm.min.ky.upperBound; 725 | else glatry[lista[j]] = a[lista[j]] + da[j]; 726 | } 727 | else { 728 | if(a[lista[j]] + da[j] > 1) glatry[lista[j]] = 1; 729 | else if(a[lista[j]] + da[j] < -1) glatry[lista[j]] = -1; 730 | else glatry[lista[j]] = a[lista[j]] + da[j]; 731 | } 732 | } 733 | 734 | else if(lista[j] == 13) { 735 | if(a[lista[j]] + da[j] < a[14] || a[lista[j]] + da[j] > a[15]) glatry[lista[j]] = a[lista[j]]; 736 | else glatry[lista[j]] = a[lista[j]] + da[j]; 737 | } 738 | 739 | else if(lista[j] == 14) { 740 | if(a[lista[j]] + da[j] > a[13]) glatry[lista[j]] = a[lista[j]]; 741 | else glatry[lista[j]] = a[lista[j]] + da[j]; 742 | } 743 | 744 | else if(lista[j] == 15) { 745 | if(a[lista[j]] + da[j] < a[13]) glatry[lista[j]] = a[lista[j]]; 746 | else glatry[lista[j]] = a[lista[j]] + da[j]; 747 | } 748 | 749 | else if(lista[j] == 16) { 750 | if(fabs(a[lista[j]] + da[j]) > 1e9) glatry[lista[j]] = a[lista[j]]; 751 | else if(a[lista[j]] + da[j] < 0.0) glatry[lista[j]] = a[lista[j]]; 752 | else glatry[lista[j]] = a[lista[j]] + da[j]; 753 | } 754 | 755 | else if(lista[j] == 17) { 756 | glatry[lista[j]] = a[lista[j]] + da[j]; 757 | glatry[lista[j]] = glatry[lista[j]] - 2 * PI * (int)(glatry[lista[j]]/(2 * PI)); 758 | } 759 | 760 | else { 761 | glatry[lista[j]] = a[lista[j]]+ da[j]; 762 | } 763 | 764 | } 765 | 766 | for (int j = mfit; j < MAX_PARAMS; j++) { 767 | glatry[lista[j]] = a[lista[j]]; 768 | } 769 | 770 | /// Adding Noise 771 | poisson = ((double) rand()) / RAND_MAX; 772 | addnoise = glochisq + (glochisq) * fabs(poisson) / 2; 773 | 774 | chisq = mrqcof(x, F, sig, ndata, glatry, lista, mfit, covar, da, &n_model, n_model_acc, addnoise); 775 | 776 | if (chisq < addnoise) { 777 | if( chisq < addnoise) alamda = alamda / NU; 778 | glochisq = chisq; 779 | *n_model_acc = n_model; 780 | for(int j = 0; j < mfit; j++) { 781 | glbeta[j] = da[j]; 782 | a[lista[j]] = glatry[lista[j]]; 783 | for(int k = 0; k < mfit; k++) alpha[j][k] = covar[j][k]; 784 | } 785 | } 786 | else { 787 | alamda = NU * alamda; 788 | chisq = glochisq; 789 | } 790 | 791 | return(alamda); 792 | 793 | } 794 | 795 | 796 | } /* ope */ 797 | -------------------------------------------------------------------------------- /Minimization.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file Minimization.h 34 | * \brief Defines functions for Levenberg-Marquadt minimization etc. 35 | * \author G. Biegelbauer, restructured by Kester Duncan 36 | */ 37 | #pragma once 38 | #ifndef MINIMIZATION_H__ 39 | #define MINIMIZATION_H__ 40 | 41 | //#include 42 | 43 | 44 | ////////////// Forward declarations to avoid large headers //////////////// 45 | namespace pcl { 46 | struct PointXYZ; 47 | template class PointCloud; 48 | 49 | } // pcl 50 | 51 | class SQParameters; 52 | 53 | ////////////////////////////////////////////////////////////////////////// 54 | 55 | 56 | //////////////////////////////////////////////////////////////////////////////////// 57 | // Types and variables used by the Levenberg Marquadt minimization procedure 58 | //////////////////////////////////////////////////////////////////////////////////// 59 | 60 | /// Maximum number of points 61 | #define MAX_POINTS 180000 62 | 63 | /// Maximum number of parameters to minimize 64 | #define MAX_PARAMS 18 65 | 66 | /// Parameter used during the minimization process 67 | #define NU 10.0 68 | 69 | /// Literal value of PI 70 | #define PI 3.141592654 71 | 72 | 73 | 74 | 75 | namespace ope { 76 | 77 | 78 | /** 79 | * \brief Defines a 3D point that would be used by the minimization routines herein 80 | */ 81 | struct MPoint { 82 | public: 83 | double point[3]; 84 | 85 | MPoint() { 86 | point[0] = 0.0; 87 | point[1] = 0.0; 88 | point[2] = 0.0; 89 | } 90 | 91 | inline double& operator[] (const int& idx) { 92 | return point[idx]; 93 | } 94 | 95 | inline double operator[] (const int& idx) const { 96 | return point[idx]; 97 | } 98 | 99 | }; 100 | 101 | 102 | typedef double glmma[MAX_PARAMS]; 103 | typedef glmma glnparam; 104 | typedef int gllista[MAX_PARAMS]; 105 | typedef double glcovar[MAX_PARAMS][MAX_PARAMS]; 106 | typedef glcovar glnalbynal; 107 | typedef glcovar glncabynca; 108 | typedef glcovar glnpbynp; 109 | typedef glcovar glnpbymp; 110 | typedef std::vector glndata; 111 | typedef std::vector glndata2; 112 | typedef gllista glnp; 113 | typedef double glmartrix[4][4]; 114 | 115 | 116 | //////////////////////////////////////////////////////////////////////////////////// 117 | //////////////////////////////////////////////////////////////////////////////////// 118 | 119 | 120 | 121 | 122 | 123 | /** 124 | * \brief Defines the minimization procedures 125 | */ 126 | struct Minimization { 127 | /** 128 | * Properties used throughout pose estimation process 129 | */ 130 | double glochisq; 131 | glmma glatry; 132 | glmma glbeta; 133 | int i_am_in_trouble; 134 | 135 | 136 | Minimization(); 137 | ~Minimization(); 138 | 139 | 140 | /** 141 | * \brief Calculates the base-10 logarithm of x 142 | */ 143 | double mylog(double x); 144 | 145 | 146 | /** 147 | * \brief Raises x to the power of y 148 | */ 149 | double mypow(double x, double y); 150 | 151 | 152 | /** 153 | * \brief Calculates the square root of x 154 | */ 155 | double sqr(double x); 156 | 157 | 158 | /** 159 | * \brief Calculates the average error of fit of superquadric parameters. 160 | * \details The error of fit is the mean of the algebraic distance calculated by the inside-outside function. 161 | */ 162 | double errorFunc(const pcl::PointCloud& cloud, SQParameters& sqParams); 163 | 164 | 165 | /** 166 | * \brief Levenberg-Marquadt cost function for superquadric fitting 167 | */ 168 | double funcs(double x, double y, double z, 169 | double rotx[11], double roty[11], double rotz[11], 170 | glnparam a, glnparam dFda); 171 | 172 | 173 | /** 174 | * \brief Utility function for computing rotations 175 | */ 176 | void precomp_rot(glnparam a, double rotx[11], double roty[11], double rotz[11]); 177 | 178 | 179 | /** 180 | * \brief Utility function for Levenberg-Marquadt minimization 181 | */ 182 | double mrqcof(const glndata2& x, glndata F, glndata sig, int ndata, 183 | glmma a, gllista lista, int mfit, glcovar alpha, glmma beta, 184 | int *n_model, int *n_model_acc, double addnoise); 185 | 186 | 187 | /** 188 | * \brief Utility function for Levenberg-Marquadt minimization 189 | */ 190 | int gaussj(glcovar a, int n, glcovar b); 191 | 192 | 193 | /** 194 | * \brief Initialize the Levenberg-Marquadt minimization 195 | */ 196 | double mrqmin_init(const glndata2& x, glndata F, glndata sig, int ndata, 197 | glmma a, gllista lista, int mfit, glcovar alpha, int nca, int *n_model_acc); 198 | 199 | 200 | /** 201 | * \brief Levenberg Marquadt minimization main procedure 202 | */ 203 | double mrqmin(SQParameters& prm, const glndata2& x, glndata F, glndata sig, 204 | int ndata, glmma a, gllista lista, int mfit, glcovar covar, glcovar alpha, 205 | double alamda, int *n_model_acc); 206 | 207 | 208 | }; 209 | 210 | 211 | } /* ope */ 212 | 213 | #endif /* MINIMIZATION_H__ */ -------------------------------------------------------------------------------- /OPECommon.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file OPESettings.h 34 | * \brief Defines macros, constants, types etc. used throughout OPE 35 | * \author Kester Duncan 36 | */ 37 | #pragma once 38 | #ifndef OPE_COMMON_H_ 39 | #define OPE_COMMON_H_ 40 | 41 | 42 | 43 | #if (defined WIN32 || defined _WIN32 || defined WINCE) 44 | #define OPE_EXPORT __declspec(dllexport) 45 | #else 46 | #define OPE_EXPORT 47 | #endif 48 | 49 | 50 | 51 | 52 | #endif /* OPE_COMMON_H_ */ -------------------------------------------------------------------------------- /OPEMain.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file Main.cpp 3 | * \brief Main entry point for Object Pose Estimation 4 | * \author Kester Duncan 5 | * 6 | * This file is the main entry point for using the object pose estimator using superquadrics by 7 | * Kester Duncan 8 | */ 9 | #if 1 10 | #include 11 | #include "SQTypes.h" 12 | #include "ObjectPoseEstimator.h" 13 | 14 | 15 | int main (int argc, char *argv[]) { 16 | ope::OPESettings settings; 17 | settings.minTgtDepth = 0.4f; 18 | settings.maxObjHeight = 2.5f; 19 | 20 | ope::ObjectPoseEstimator estimator(settings); 21 | ope::SQParameters sqParams = estimator.run(); 22 | 23 | std::cin.get(); 24 | std::cin.get(); 25 | 26 | return 0; 27 | } 28 | 29 | 30 | #endif -------------------------------------------------------------------------------- /OPESettings.cpp: -------------------------------------------------------------------------------- 1 | #include "OPESettings.h" 2 | 3 | namespace ope { 4 | 5 | OPESettings::OPESettings() : minTgtDepth(0.2f), maxTgtDepth(1.8f), minObjHeight(0.01f), maxObjHeight(0.50f), 6 | objVoxelSize(0.003f), maxVoxelSize(0.03f), minVoxelSize(0.003f), 7 | verbose(false), allowTapering(false), doDebug(false), minIterations(20) { 8 | 9 | } 10 | 11 | 12 | OPESettings& OPESettings::operator= (const OPESettings& other) { 13 | this->allowTapering = other.allowTapering; 14 | this->doDebug = other.doDebug; 15 | this->verbose = other.verbose; 16 | this->minObjHeight = other.minObjHeight; 17 | this->maxObjHeight = other.maxObjHeight; 18 | this->minTgtDepth = other.minTgtDepth; 19 | this->maxTgtDepth = other.maxTgtDepth; 20 | this->minVoxelSize = other.minVoxelSize; 21 | this->maxVoxelSize = other.maxVoxelSize; 22 | this->minIterations = other.minIterations; 23 | this->objVoxelSize = other.objVoxelSize; 24 | 25 | return *this; 26 | 27 | } 28 | 29 | 30 | } -------------------------------------------------------------------------------- /OPESettings.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file OPESettings.h 34 | * \brief Defines the settings object used throughout OPE 35 | * \author Kester Duncan 36 | */ 37 | #pragma once 38 | #ifndef OPE_SETTINGS_H_ 39 | #define OPE_SETTINGS_H_ 40 | 41 | 42 | #include "OPECommon.h" 43 | 44 | /** \namespace ope 45 | * Namespace where all the Object Pose Estimation functionality resides 46 | */ 47 | namespace ope { 48 | 49 | /** 50 | * \brief User-customizable program settings for pose estimation process 51 | */ 52 | struct OPESettings { 53 | /// Minimum depth value for target scene area. Used by the PassThrough filter 54 | float minTgtDepth; 55 | 56 | /// Maximum depth value for target scene area 57 | float maxTgtDepth; 58 | 59 | /// Minimum height of object hypotheses in the scene 60 | float minObjHeight; 61 | 62 | /// Maximum height of object hypotheses in the scene 63 | float maxObjHeight; 64 | 65 | /// Maximum voxel size for superquadric fitting 66 | float maxVoxelSize; 67 | 68 | /// Minimum voxel size for superquadric fitting 69 | float minVoxelSize; 70 | 71 | /// Determines whether status updates are output 72 | bool verbose; 73 | 74 | /// Determines the amount of error minimization iterations for superquadric fitting 75 | int minIterations; 76 | 77 | /// Determines whether superquadric shape tapering is allowed when estimating pose 78 | bool allowTapering; 79 | 80 | /// Object voxel size for downsampling 81 | float objVoxelSize; 82 | 83 | /// Show a debugging viewer when point cloud is captured 84 | bool doDebug; 85 | 86 | /// Default Constructor 87 | OPE_EXPORT OPESettings(); 88 | 89 | /// Overloaded assignment operator 90 | OPE_EXPORT OPESettings& operator= (const OPESettings& other); 91 | 92 | }; 93 | 94 | } /* ope */ 95 | 96 | 97 | 98 | #endif /* OPE_SETTINGS_H_ */ -------------------------------------------------------------------------------- /OPEUtils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "TableObjectModeler.h" 11 | #include "OPEUtils.h" 12 | 13 | using namespace ope; 14 | 15 | 16 | Utils::Utils() { 17 | } 18 | 19 | 20 | Utils::~Utils() { 21 | } 22 | 23 | 24 | void Utils::convertPointCloud(const pcl::PointCloud& src, pcl::PointCloud& tgt) { 25 | pcl::PointXYZRGB nanPoint; 26 | nanPoint.getVector4fMap().setConstant (std::numeric_limits::quiet_NaN()); 27 | tgt.points.resize((int) (src.width * src.height), nanPoint); 28 | tgt.is_dense = src.is_dense; 29 | tgt.width = src.width; 30 | tgt.height = src.height; 31 | tgt.header = src.header; 32 | 33 | for (size_t i = 0; i < src.points.size(); i++) { 34 | pcl::PointXYZRGB pt; 35 | pt.x = src.points[i].x; 36 | pt.y = src.points[i].y; 37 | pt.z = src.points[i].z; 38 | pt.r = src.points[i].r; 39 | pt.g = src.points[i].g; 40 | pt.b = src.points[i].b; 41 | 42 | tgt.points.push_back(pt); 43 | } 44 | } 45 | 46 | 47 | void Utils::transformPointCloud(pcl::PointCloud& cloud) { 48 | for (size_t i = 0; i < cloud.size(); ++i) { 49 | pcl::PointXYZRGB p = cloud.points[i]; 50 | 51 | cloud.points[i].x = p.z; 52 | cloud.points[i].y = -p.x; 53 | cloud.points[i].z = p.y; 54 | } 55 | 56 | } 57 | 58 | 59 | void Utils::printCurrentDateTime() { 60 | std::ostringstream msg; 61 | const boost::posix_time::ptime now = boost::posix_time::second_clock::local_time(); 62 | std::cout << "+-------------------------------------------+" << std::endl; 63 | std::cout << " " << now.date() << std::endl; 64 | std::cout << " " << now.time_of_day() << std::endl; 65 | std::cout << "+-------------------------------------------+" << std::endl << std::endl; 66 | } 67 | 68 | 69 | size_t Utils::getDesiredObject(pcl::PointCloud::ConstPtr ptCloudPtr, const std::vector& boxes) { 70 | srand(time(NULL)); 71 | size_t desiredObjIdx = 0; 72 | boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("Objects - In the command window, enter the object's index")); 73 | pcl::visualization::PointCloudColorHandlerRGBField rgb(ptCloudPtr); 74 | viewer->setBackgroundColor(0, 0, 0); 75 | viewer->addPointCloud (ptCloudPtr, rgb, "Cloud"); 76 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "Cloud"); 77 | 78 | for (size_t i = 0; i < boxes.size(); ++i) { 79 | std::string objId("Object_"); 80 | std::string cubeId("Cube_"); 81 | objId += boost::lexical_cast(i); 82 | cubeId += boost::lexical_cast(i); 83 | 84 | viewer->addCube(boxes[i].minX, boxes[i].maxX, boxes[i].minY, boxes[i].maxY, boxes[i].minZ, boxes[i].maxZ, 1.0, 1.0, 0.0, cubeId); 85 | viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3.0, cubeId); 86 | pcl::PointXYZ p(boxes[i].minX, boxes[i].minY, boxes[i].minZ); 87 | viewer->addText3D(objId, p, 0.025); 88 | } 89 | viewer->initCameraParameters(); 90 | viewer->resetCameraViewpoint("Cloud"); 91 | 92 | while (!viewer->wasStopped()) { 93 | viewer->spinOnce(100); 94 | boost::this_thread::sleep (boost::posix_time::microseconds(10)); 95 | } 96 | 97 | std::cout << ">> Enter the index of the object you wish to process: "; 98 | std::cin >> desiredObjIdx; 99 | while (desiredObjIdx < 0 || desiredObjIdx >= boxes.size()) { 100 | std::cout << "\n>> Invalid index, enter a correct object index: "; 101 | std::cin >> desiredObjIdx; 102 | } 103 | 104 | return desiredObjIdx; 105 | 106 | } -------------------------------------------------------------------------------- /OPEUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file Utils.h 34 | * \author Kester Duncan 35 | */ 36 | #pragma once 37 | #ifndef OPE_UTILS_H__ 38 | #define OPE_UTILS_H__ 39 | 40 | 41 | ////////////// Forward declarations to avoid large headers //////////////// 42 | namespace pcl { 43 | class PointXYZ; 44 | class PointXYZRGB; 45 | class PointXYZRGBA; 46 | template class PointCloud; 47 | 48 | } // pcl 49 | 50 | 51 | 52 | namespace ope { 53 | 54 | // Forward declaration to avoid including header 55 | class BoundingBox; 56 | 57 | 58 | /** 59 | * \brief Performs frequently used utility functions 60 | * \author Kester Duncan 61 | */ 62 | class Utils 63 | { 64 | public: 65 | Utils(); 66 | ~Utils(); 67 | 68 | 69 | /** 70 | * \brief Convert between pcl point cloud types 71 | * \details Converts from pcl::PointXYZRGBA to pcl::PointXYZRGB 72 | * \param - input pcl::PointXYZRGBA point cloud 73 | * \param - output pcl::PointXYZRGB point cloud 74 | */ 75 | static void convertPointCloud(const pcl::PointCloud& src, pcl::PointCloud& tgt); 76 | 77 | 78 | /** 79 | * \brief Transform an XYZRGB point cloud from the Kinect optical frame to world coordinate frame 80 | * \param - the cloud to be transformed 81 | */ 82 | static void transformPointCloud(pcl::PointCloud& cloud); 83 | 84 | 85 | /** 86 | * \brief Prints the current local time to the output stream 87 | */ 88 | static void printCurrentDateTime(); 89 | 90 | 91 | /** 92 | * \brief Displays a pointcloud with highlighted objects in order to determine the user's object of choice 93 | */ 94 | static size_t getDesiredObject(pcl::PointCloud::ConstPtr ptCloudPtr, const std::vector& boxes); 95 | 96 | }; 97 | 98 | 99 | } /* ope */ 100 | 101 | 102 | #endif /* OPE_UTILS_H__ */ 103 | 104 | -------------------------------------------------------------------------------- /ObjectPoseEstimation.vcxproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | 14 | {0889DB3B-F63B-400E-9109-4E8C2120A55F} 15 | ObjectPoseEstimation 16 | 17 | 18 | 19 | Application 20 | true 21 | MultiByte 22 | 23 | 24 | DynamicLibrary 25 | false 26 | true 27 | MultiByte 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | $(SolutionDir)\libs\ 41 | 42 | 43 | 44 | Level3 45 | Disabled 46 | C:\Program Files %28x86%29\boost\boost_1_51;C:\Program Files %28x86%29\Eigen\include;C:\Program Files %28x86%29\OpenNI\Include;C:\Program Files %28x86%29\VTK 5.8.0\include\vtk-5.8;C:\Program Files %28x86%29\flann\include;C:\Program Files %28x86%29\PCL 1.6.0\include\pcl-1.6 47 | 48 | 49 | true 50 | C:\Program Files %28x86%29\boost\boost_1_51\lib;C:\Program Files %28x86%29\VTK 5.8.0\lib\vtk-5.8;C:\Program Files %28x86%29\OpenNI\Lib;C:\Program Files %28x86%29\PCL 1.6.0\lib;%(AdditionalLibraryDirectories) 51 | opengl32.lib;vtksys.lib;vtkjpeg.lib;vtkpng.lib;vtktiff.lib;vtkzlib.lib;vtkexpat.lib;vtkIO.lib;vtkCommon.lib;vtkFiltering.lib;vtkGraphics.lib;vtkImaging.lib;vtkHybrid.lib;vtkRendering.lib;pcl_common_release.lib;pcl_io_release.lib;pcl_visualization_release.lib;pcl_filters_release.lib;pcl_kdtree_release.lib;pcl_octree_release.lib;pcl_registration_release.lib;pcl_sample_consensus_release.lib;pcl_search_release.lib;pcl_segmentation_release.lib;pcl_features_release.lib;pcl_keypoints_release.lib;pcl_surface_release.lib;%(AdditionalDependencies) 52 | 53 | 54 | 55 | 56 | Level3 57 | C:\Program Files %28x86%29\boost\boost_1_51;C:\Program Files %28x86%29\Eigen\include;C:\Program Files %28x86%29\OpenNI\Include;C:\Program Files %28x86%29\VTK 5.8.0\include\vtk-5.8;C:\Program Files %28x86%29\flann\include;C:\Program Files %28x86%29\PCL 1.6.0\include\pcl-1.6 58 | true 59 | %(PreprocessorDefinitions) 60 | 61 | 62 | true 63 | true 64 | true 65 | 0.3.0 66 | C:\Program Files %28x86%29\boost\boost_1_51\stage\lib;C:\Program Files %28x86%29\VTK 5.8.0\lib\vtk-5.8;C:\Program Files %28x86%29\OpenNI\Lib;C:\Program Files %28x86%29\PCL 1.6.0\lib;%(AdditionalLibraryDirectories) 67 | opengl32.lib;vtksys.lib;vtkjpeg.lib;vtkpng.lib;vtktiff.lib;vtkzlib.lib;vtkexpat.lib;vtkIO.lib;vtkCommon.lib;vtkFiltering.lib;vtkGraphics.lib;vtkImaging.lib;vtkHybrid.lib;vtkRendering.lib;pcl_common_release.lib;pcl_io_release.lib;pcl_visualization_release.lib;pcl_filters_release.lib;pcl_kdtree_release.lib;pcl_octree_release.lib;pcl_registration_release.lib;pcl_sample_consensus_release.lib;pcl_search_release.lib;pcl_segmentation_release.lib;pcl_features_release.lib;pcl_keypoints_release.lib;pcl_surface_release.lib;%(AdditionalDependencies) 68 | 69 | 70 | true 71 | 72 | 73 | libboost_thread-vc100-mt-s-1_51.lib;pcl_common_release.lib;pcl_kdtree_release.lib;pcl_search_release.lib;pcl_filters_release.lib;pcl_sample_consensus_release.lib;pcl_segmentation_release.lib;pcl_visualization_release.lib;pcl_features_release.lib;pcl_surface_release.lib;%(AdditionalDependencies) 74 | C:\Program Files %28x86%29\boost\boost_1_51\stage\lib;C:\Program Files %28x86%29\VTK 5.8.0\lib\vtk-5.8;C:\Program Files %28x86%29\OpenNI\Lib;C:\Program Files %28x86%29\PCL 1.6.0\lib;%(AdditionalLibraryDirectories) 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | -------------------------------------------------------------------------------- /ObjectPoseEstimation.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | 14 | 15 | Header Files 16 | 17 | 18 | Header Files 19 | 20 | 21 | Header Files 22 | 23 | 24 | Header Files 25 | 26 | 27 | Header Files 28 | 29 | 30 | Header Files 31 | 32 | 33 | Header Files 34 | 35 | 36 | Header Files 37 | 38 | 39 | Header Files 40 | 41 | 42 | Header Files 43 | 44 | 45 | Header Files 46 | 47 | 48 | Header Files 49 | 50 | 51 | Header Files 52 | 53 | 54 | Header Files 55 | 56 | 57 | 58 | 59 | Source Files 60 | 61 | 62 | Source Files 63 | 64 | 65 | Source Files 66 | 67 | 68 | Source Files 69 | 70 | 71 | Source Files 72 | 73 | 74 | Source Files 75 | 76 | 77 | Source Files 78 | 79 | 80 | Source Files 81 | 82 | 83 | Source Files 84 | 85 | 86 | -------------------------------------------------------------------------------- /ObjectPoseEstimator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "OPESettings.h" 11 | #include "SQTypes.h" 12 | #include "OPEUtils.h" 13 | #include "Minimization.h" 14 | #include "SQFitting.h" 15 | #include "PointCloudCapture.h" 16 | #include "TableObjectModeler.h" 17 | #include "ObjectPoseEstimator.h" 18 | 19 | 20 | namespace ope { 21 | 22 | ObjectPoseEstimator::ObjectPoseEstimator() : settings(OPESettings()) { 23 | 24 | } 25 | 26 | 27 | ObjectPoseEstimator::ObjectPoseEstimator(const OPESettings& settings) { 28 | this->settings = settings; 29 | 30 | } 31 | 32 | 33 | SQParameters ObjectPoseEstimator::calculateObjectPose(pcl::PointCloud& selectedObjectPtCloud) { 34 | // Main object instance for superquadric fitting 35 | SQFitting fittingProcess; 36 | 37 | // The initial XYZ point cloud that is processes for pose estimation 38 | pcl::PointCloud cloud; 39 | 40 | // The downsampled XYZ point cloud that is used in the multiscale voxelization scheme 41 | pcl::PointCloud downsampledCloud; 42 | 43 | // A pointer to the initial XYZ point cloud used during voxelization 44 | pcl::PointCloud::Ptr cloudPtr (new pcl::PointCloud()); 45 | 46 | // Initial superquadric parameters based on the cloud properties 47 | SQParameters initParams; 48 | 49 | // The final (best) superquadric parameters that are found 50 | SQParameters bestParams; 51 | 52 | // Estimate of the superquadric parameters used for bootstrapping the multiscale voxelization scheme 53 | SQParameters sqEstimate; 54 | 55 | // A record of the initial number of points before repeated downsampling 56 | int numberOfPoints = 0; 57 | 58 | /* 59 | * The point cloud must be transformed from the Kinect optical frame to the 60 | * world coordinate frame. Additional transformations may have to be done depending 61 | * on the application, but this is left up to the user. 62 | */ 63 | Utils::transformPointCloud(selectedObjectPtCloud); 64 | 65 | /* 66 | * The captured XYZRGB point cloud must be converted to an XYZ cloud 67 | * in order to perform pose estimation 68 | */ 69 | for (size_t i = 0; i < selectedObjectPtCloud.size(); ++i) { 70 | pcl::PointXYZ p; 71 | p.x = selectedObjectPtCloud[i].x; 72 | p.y = selectedObjectPtCloud[i].y; 73 | p.z = selectedObjectPtCloud[i].z; 74 | 75 | cloud.points.push_back(p); 76 | numberOfPoints++; 77 | } 78 | cloud.width = numberOfPoints; 79 | cloud.height = 1; 80 | *cloudPtr = cloud; 81 | 82 | /* 83 | * The voxel downsampling parameters must be initialized 84 | */ 85 | const int NUM_SCALES = 4; // 4 is our set number 86 | const float MAX_VOXEL = settings.maxVoxelSize; 87 | const float MIN_VOXEL = settings.minVoxelSize; 88 | const float SCALE_DIFF = (MAX_VOXEL - MIN_VOXEL) / (float) NUM_SCALES; 89 | float gridSizes[NUM_SCALES]; 90 | double totalElapsedTime = 0; 91 | 92 | for (int s = 0; s < NUM_SCALES; s++) { 93 | gridSizes[s] = MAX_VOXEL - (s * SCALE_DIFF); 94 | } 95 | 96 | pcl::PointCloud::Ptr smoothCloudPtr (new pcl::PointCloud); 97 | pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); 98 | pcl::MovingLeastSquares mls; 99 | mls.setComputeNormals (false); 100 | 101 | // Set parameters 102 | mls.setInputCloud (cloudPtr); 103 | mls.setPolynomialFit (true); 104 | mls.setSearchMethod (tree); 105 | mls.setSearchRadius (0.03); 106 | 107 | // Reconstruct 108 | mls.process (*smoothCloudPtr); 109 | 110 | /* 111 | * Calculate first error differences 112 | */ 113 | /// Variables pertaining to multi-scale voxelization algorithm 114 | double errorThreshold = 2.0; 115 | double errorDiff = 1000.0; 116 | double errorValue = 0; 117 | double prevErrorValue = 0; 118 | 119 | fittingProcess.estimateInitialParameters(*smoothCloudPtr, sqEstimate); 120 | errorValue = fittingProcess.qualityOfFit(*smoothCloudPtr, sqEstimate); 121 | errorDiff = abs(prevErrorValue - errorValue); 122 | prevErrorValue = errorValue; 123 | 124 | if (settings.verbose == true) { 125 | std::cout << ">> Performing object pose estimation using Superquadrics\n"; 126 | } 127 | 128 | /* 129 | * Multi-scale voxelization for pose estimation 130 | */ 131 | for (int j = 0; j < NUM_SCALES && errorDiff >= errorThreshold; j++) { 132 | if (j != NUM_SCALES - 1) { 133 | pcl::VoxelGrid grid; 134 | grid.setInputCloud (smoothCloudPtr); 135 | grid.setLeafSize (gridSizes[j], gridSizes[j], gridSizes[j]); 136 | grid.filter (downsampledCloud); 137 | 138 | } else { 139 | downsampledCloud = *smoothCloudPtr; 140 | } 141 | 142 | /* 143 | * Estimate the dimensions of cloud (in order to get a more accurate fit) 144 | */ 145 | fittingProcess.estimateInitialParameters(downsampledCloud, sqEstimate); 146 | 147 | /* 148 | * Initialize the superquadric parameters based on the cloud dimensions 149 | */ 150 | initParams.min.iterations = settings.minIterations; 151 | initParams.min.a1.type = BOUNDED; 152 | initParams.min.a1.lowerBound = 0.020f; 153 | initParams.min.a1.upperBound = sqEstimate.a1 + 0.015f; 154 | initParams.a1 = 0.05f; 155 | 156 | initParams.min.a2.type = BOUNDED; 157 | initParams.min.a2.lowerBound = 0.020f; 158 | initParams.min.a2.upperBound = sqEstimate.a2 + 0.015f; 159 | initParams.a2 = 0.05f; 160 | 161 | initParams.min.a3.type = BOUNDED; 162 | initParams.min.a3.lowerBound = 0.020f; 163 | initParams.min.a3.upperBound = sqEstimate.a3 + 0.015f; 164 | initParams.a3 = 0.05f; 165 | 166 | initParams.min.e1.type = BOUNDED; 167 | initParams.min.e1.lowerBound = 0.1f; 168 | initParams.min.e1.upperBound = 1.0f; 169 | initParams.e1 = 1.0f; 170 | 171 | initParams.min.e2.type = BOUNDED; 172 | initParams.min.e2.lowerBound = 0.1f; 173 | initParams.min.e2.upperBound = 1.0f; 174 | initParams.e2 = 1.0f; 175 | 176 | initParams.min.phi.type = UNLIMITED; 177 | initParams.min.theta.type = UNLIMITED; 178 | initParams.min.psi.type = UNLIMITED; 179 | initParams.min.phi.value = 1.0f; 180 | initParams.min.theta.value = 1.0f; 181 | initParams.min.psi.value = 1.0f; 182 | 183 | initParams.min.px.type = UNLIMITED; 184 | initParams.min.py.type = UNLIMITED; 185 | initParams.min.pz.type = UNLIMITED; 186 | 187 | if (settings.allowTapering) { 188 | initParams.min.kx.type = BOUNDED; 189 | initParams.min.kx.lowerBound = -0.25f; 190 | initParams.min.kx.upperBound = 0.25f; 191 | initParams.min.kx.value = 0.0f; 192 | initParams.kx = 1.0f; 193 | 194 | initParams.min.ky.type = BOUNDED; 195 | initParams.min.ky.lowerBound = -0.25f; 196 | initParams.min.ky.upperBound = 0.25; 197 | initParams.min.ky.value = 0.0f; 198 | initParams.ky = 1.0f; 199 | 200 | } else { 201 | initParams.min.kx.type = UNCHANGED; 202 | initParams.min.kx.value = 0.0f; 203 | initParams.min.ky.type = UNCHANGED; 204 | initParams.min.ky.value = 0.0f; 205 | 206 | } 207 | 208 | std::stringstream os; 209 | double elapsedTime = 0.0; 210 | { 211 | boost::progress_timer timer(os); 212 | fittingProcess.performShapeFitting(downsampledCloud, initParams, bestParams); 213 | } 214 | 215 | if (!(os >> elapsedTime)) { 216 | elapsedTime = atof(os.str().c_str()); 217 | } 218 | 219 | totalElapsedTime += elapsedTime; 220 | 221 | errorValue = fittingProcess.qualityOfFit(cloud, bestParams); 222 | errorDiff = abs(prevErrorValue - errorValue); 223 | prevErrorValue = errorValue; 224 | 225 | /* 226 | * The parameters found at this level are used to initialize the successive one 227 | */ 228 | bestParams.copyTo(initParams); 229 | downsampledCloud.clear(); 230 | } 231 | 232 | initParams.copyTo(bestParams); 233 | 234 | if (settings.verbose == true) { 235 | std::cout <<">> Superquadric fitting processing time: " << totalElapsedTime << " seconds" << std::endl; 236 | } 237 | 238 | return bestParams; 239 | } 240 | 241 | 242 | SQParameters ObjectPoseEstimator::run() { 243 | SQParameters sqParams; 244 | PointCloudCapture cap; 245 | pcl::PointCloud::Ptr cloudPtr (new pcl::PointCloud); 246 | bool detectedObjects; 247 | size_t desiredObjIdx = 0; 248 | 249 | /* 250 | * Capture point cloud 251 | */ 252 | cap.run(*cloudPtr); 253 | 254 | 255 | /* 256 | * Generate object models based on extracted clusters 257 | */ 258 | ObjectModelGenerator generator(cloudPtr); 259 | detectedObjects = generator.generateObjectModels(settings); 260 | 261 | 262 | /* 263 | * Choose the object whose pose (position and orientation) you wish to calculate 264 | */ 265 | if (detectedObjects) { 266 | desiredObjIdx = Utils::getDesiredObject(cloudPtr, generator.getBoundingBoxes()); 267 | 268 | /* 269 | * Calculate the object pose using Superquadrics 270 | */ 271 | sqParams = ObjectPoseEstimator::calculateObjectPose(generator.objects[desiredObjIdx].objectCloud); 272 | std::cout << sqParams; 273 | 274 | } else { 275 | if (settings.verbose) { 276 | std::cout << ">> NO OBJECTS WERE DETECTED!" << std::endl; 277 | } 278 | } 279 | 280 | return sqParams; 281 | } 282 | 283 | 284 | } // ope -------------------------------------------------------------------------------- /ObjectPoseEstimator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file ObjectPoseEstimator.h 34 | * \brief Performs object pose estimation using superquadrics 35 | * \author Kester Duncan 36 | */ 37 | #pragma once 38 | #ifndef OBJECT_POSE_ESTIMATOR_H__ 39 | #define OBJECT_POSE_ESTIMATOR_H__ 40 | 41 | #include "OPESettings.h" 42 | 43 | 44 | namespace pcl { 45 | 46 | struct PointXYZRGB; 47 | template class PointCloud; 48 | 49 | } // pcl 50 | 51 | 52 | 53 | /// Namespace \a ope that contains all the functions and types relevant for estimating an object's pose 54 | namespace ope { 55 | 56 | /// Forward declarations to avoid unnecessary header file includes 57 | class SQFitting; 58 | 59 | 60 | /** 61 | * \brief Performs object pose estimation using the parametric superquadric shape model 62 | * \details Superquadrics are a family of parametric shapes that include superellipsoids, 63 | * supertoroids, and superhyperboloids with one and two parts. They are appealing for 64 | * robotic applications by nature of their definition. We focus on the superellipsoid which 65 | * is useful for a volumetric part-based description. Given the parameters that define a 66 | * superquadric, the shape and pose information can be easily extracted as well as volumes 67 | * and moments of inertia. They are compact in shape and have a closed surface. Moreover, 68 | * superquadrics exhibit tri-axis symmetry, which is a characteristic well approximated by 69 | * many household objects. 70 | * 71 | * Superquadrics can be defined in an object centered coordinate system with five variables 72 | * and in a general coordinate system by eleven independent variables. The variables 73 | * \f$ a_1, a_2, a_3 \f$ are the scaling dimensions along the x, y, and z 74 | * axes of the superquadric, \f$ e1, e2 \f$ are the factors which determine the 75 | * superquadric's shape ranging from from 0.1 to 1, and 76 | * \f$ (n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z) \f$ are the twelve parameters 77 | * of the homogeneous transformation matrix that is a result of a rotation and translation 78 | * of the world coordinate frame. 79 | */ 80 | class ObjectPoseEstimator 81 | { 82 | private: 83 | OPESettings settings; 84 | 85 | public: 86 | /** 87 | * \brief Calculate the pose of the object represented by the point cloud provided 88 | * \details The pose is estimated by using the algorithm outlined in the ICRA 2013 paper 89 | * \b "Multi-scale Superquadric Fitting for Efficient Shape and Pose Recovery of Unknown Objects" 90 | * by Kester Duncan 91 | * http://www.cse.usf.edu/~kkduncan 92 | * 93 | */ 94 | OPE_EXPORT SQParameters calculateObjectPose(pcl::PointCloud& selectedObjectPtCloud); 95 | 96 | 97 | /** 98 | * \brief Run the pose estimation algorithm 99 | * \details This function executes as follows: Capture a point cloud, extract the table objects 100 | * from the point cloud, present a viewing window to the user in order for them to select the 101 | * target object point cloud using its index, perform pose estimation on the object point cloud, 102 | * and finally return the results in a \a SQParameters object. * 103 | * \return An \a SQParameters object containing the 13 superquadric parameters. 104 | */ 105 | OPE_EXPORT SQParameters run(); 106 | 107 | /// Default constructor 108 | OPE_EXPORT ObjectPoseEstimator(); 109 | 110 | /// Constructor with settings 111 | OPE_EXPORT ObjectPoseEstimator(const OPESettings& settings); 112 | 113 | }; 114 | 115 | 116 | } // ope 117 | 118 | #endif /* OBJECT_POSE_ESTIMATOR_H__ */ -------------------------------------------------------------------------------- /Plane.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Plane.h" 3 | 4 | using namespace ope; 5 | 6 | 7 | /// Normalize an XYZ vector 8 | static void normalize(pcl::PointXYZ& v) { 9 | float denom = 1.0f / std::sqrtf((v.x * v.x) + (v.y * v.y) + (v.z * v.z)); 10 | 11 | v.x *= denom; 12 | v.y *= denom; 13 | v.z *= denom; 14 | } 15 | 16 | 17 | Plane :: Plane(const pcl::PointXYZ& normal, const pcl::PointXYZ& p) { 18 | a = normal.x; 19 | b = normal.y; 20 | c = normal.z; 21 | d = -(normal.x * p.x + normal.y * p.y + normal.z *p.z); 22 | } 23 | 24 | pcl::PointXYZ Plane :: normal() const { 25 | pcl::PointXYZ n(a,b,c); 26 | normalize(n); 27 | return n; 28 | } 29 | 30 | bool Plane :: isValid() const { 31 | const float eps = 1e-15; 32 | return (std::abs(a) > eps) || (std::abs(b) > eps) || (std::abs(c) > eps); 33 | } 34 | 35 | 36 | pcl::PointXYZ Plane :: intersectionWithLine (const pcl::PointXYZ& p1, const pcl::PointXYZ& p2) const { 37 | double u = a * p1.x + b * p1.y + c * p1.z + d; 38 | u /= a * (p1.x - p2.x) + b * (p1.y - p2.y) + c * (p1.z - p2.z); 39 | 40 | pcl::PointXYZ r; 41 | r.x = p1.x + u * (p2.x - p1.x); 42 | r.y = p1.y + u * (p2.y - p1.y); 43 | r.z = p1.z + u * (p2.z - p1.z); 44 | 45 | return r; 46 | } 47 | 48 | 49 | float Plane :: distanceToPlane(const pcl::PointXYZ& p) const { 50 | float v = a * p.x + b * p.y + c * p.z + d; 51 | v /= sqrt(a * a + b * b + c * c); 52 | return std::abs(v); 53 | } -------------------------------------------------------------------------------- /Plane.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file Plane.h 34 | * \author Kester Duncan 35 | * \note Adapted from ntk's Plane object 36 | */ 37 | #pragma once 38 | #ifndef PLANE_H__ 39 | #define PLANE_H__ 40 | 41 | 42 | // Forward declaration 43 | namespace pcl { 44 | class PointXYZ; 45 | 46 | } 47 | 48 | 49 | namespace ope 50 | { 51 | 52 | 53 | /** 54 | * \brief Defines the properties of a plane 55 | */ 56 | class Plane { 57 | /// Plane parameters 58 | double a, b, c, d; 59 | 60 | public: 61 | Plane(double a, double b, double c, double d) 62 | : a(a), b(b), c(c), d(d) 63 | {} 64 | 65 | /** 66 | * \brief Construct a plane from a normal vector and a point. 67 | */ 68 | Plane(const pcl::PointXYZ& normal, const pcl::PointXYZ& p); 69 | 70 | /** 71 | * \brief Default Constructor 72 | */ 73 | Plane() : a(0), b(0), c(0), d(0) 74 | {} 75 | 76 | /** 77 | * \brief Determines whether or not this is a valid plane 78 | * \return true if it is valid 79 | */ 80 | bool isValid() const; 81 | 82 | /** 83 | * \brief Gets the normal vector that defines this plane 84 | * \returns the x, y, & z values of the plane's normal 85 | */ 86 | pcl::PointXYZ normal() const; 87 | 88 | /** 89 | * \brief Sets the parameters of the plane 90 | */ 91 | void set (double a_, double b_, double c_, double d_) { 92 | a = a_; 93 | b = b_; 94 | c = c_; 95 | d = d_; 96 | } 97 | 98 | /** 99 | * \brief Determines whether this plane intersects with the specified line given by the parameters 100 | * \param the first point that defines the line 101 | * \param the second point that defines the line 102 | */ 103 | pcl::PointXYZ intersectionWithLine (const pcl::PointXYZ& p1, const pcl::PointXYZ& p2) const; 104 | 105 | 106 | /** 107 | * \brief Determines a point's distance from the plane 108 | * \return Distance from the plane 109 | */ 110 | float distanceToPlane(const pcl::PointXYZ& p) const; 111 | 112 | 113 | }; 114 | 115 | 116 | } // ope 117 | 118 | #endif /* PLANE_H__ */ 119 | -------------------------------------------------------------------------------- /PointCloudCapture.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "OPESettings.h" 5 | #include "OPEUtils.h" 6 | 7 | #include "PointCloudCapture.h" 8 | 9 | using namespace ope; 10 | 11 | PointCloudCapture::PointCloudCapture() : ptCloudPtr(new pcl::PointCloud), 12 | filteredPtCloudPtr(new pcl::PointCloud) { 13 | 14 | } 15 | 16 | 17 | PointCloudCapture::~PointCloudCapture() { 18 | 19 | } 20 | 21 | 22 | void PointCloudCapture::cloudCallback(const pcl::PointCloud::ConstPtr &cloud) { 23 | ptCloudPtr = cloud; 24 | } 25 | 26 | 27 | void PointCloudCapture::run(pcl::PointCloud& ptCloud, const OPESettings& settings) { 28 | pcl::Grabber* grabber = new pcl::OpenNIGrabber(); 29 | 30 | boost::function::ConstPtr&)> f = 31 | boost::bind (&PointCloudCapture::cloudCallback, this, _1); 32 | 33 | if (settings.verbose) { 34 | Utils::printCurrentDateTime(); 35 | std::cout << ">> Capturing point cloud using the Kinect" << std::endl; 36 | } 37 | 38 | grabber->registerCallback (f); 39 | grabber->start (); 40 | 41 | // Pause for 30 milliseconds to ensure that a point cloud is captured 42 | boost::this_thread::sleep (boost::posix_time::millisec (30)); 43 | 44 | grabber->stop (); 45 | 46 | pcl::PassThrough pass; 47 | pass.setInputCloud (ptCloudPtr); 48 | pass.setFilterFieldName ("z"); 49 | pass.setFilterLimits (settings.minTgtDepth, settings.maxTgtDepth); 50 | pass.filter (*filteredPtCloudPtr); 51 | 52 | Utils::convertPointCloud(*filteredPtCloudPtr, ptCloud); 53 | 54 | if (settings.verbose) { 55 | std::cout << ">>\t ...done" << std::endl; 56 | } 57 | 58 | // Only for debugging purposes 59 | if (settings.doDebug) { 60 | boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("Debugging Viewer")); 61 | pcl::visualization::PointCloudColorHandlerRGBField rgb(filteredPtCloudPtr); 62 | viewer->setBackgroundColor(0, 0, 0); 63 | viewer->addPointCloud (filteredPtCloudPtr, rgb, "Filtered Cloud"); 64 | viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "Filtered Cloud"); 65 | viewer->initCameraParameters(); 66 | viewer->resetCameraViewpoint("Filtered Cloud"); 67 | 68 | while (!viewer->wasStopped()) { 69 | viewer->spinOnce(100); 70 | boost::this_thread::sleep (boost::posix_time::microseconds(10)); 71 | } 72 | } 73 | 74 | } 75 | -------------------------------------------------------------------------------- /PointCloudCapture.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file PointCloudCapture.h 34 | * \author Kester Duncan 35 | */ 36 | #pragma once 37 | #ifndef POINTCLOUDCAPTURE_H__ 38 | #define POINTCLOUDCAPTURE_H__ 39 | 40 | #include 41 | #include 42 | 43 | namespace ope { 44 | 45 | /** 46 | * \brief Captures XYZRGB point clouds from the Kinect 47 | * \note The coordinate system for the captured point clouds is as follows: 48 | * x-axis -> right, y-axis -> down, z-axis -> points into scene. 49 | * ________+x 50 | * |\ 51 | * | \ 52 | * | \ 53 | * | \ 54 | * | \ 55 | * +y +z 56 | * 57 | * Also, a PassThrough filter is applied to the captured point cloud so that the 58 | * target area is within a specified range which is adjustable 59 | */ 60 | class PointCloudCapture 61 | { 62 | private: 63 | pcl::PointCloud::ConstPtr ptCloudPtr; 64 | pcl::PointCloud::Ptr filteredPtCloudPtr; 65 | 66 | public: 67 | PointCloudCapture(); 68 | ~PointCloudCapture(); 69 | 70 | /** 71 | * \brief Captures an XYZRGBA point cloud from the Kinect and stores an XYZRGB cloud 72 | * \param - holds the captured PointXYZRGB point cloud 73 | */ 74 | void run(pcl::PointCloud& ptCloud, const OPESettings& settings = OPESettings()); 75 | 76 | 77 | /// \brief Callback function that is used to read XYZRGB point clouds from the Kinect 78 | void cloudCallback(const pcl::PointCloud::ConstPtr &cloud); 79 | }; 80 | 81 | 82 | } /* end of namespace ope */ 83 | 84 | 85 | #endif /* POINTCLOUDCAPTURE_H__ */ 86 | 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ObjectPoseEstimation 2 | ==================== 3 | 4 | Estimates the position and orientation of an object represented as a 3D point cloud using superquadrics. 5 | This is the implementation of the algorithm detailed in the ICRA 2013 submission by K. Duncan et al. entitled 6 | "Multi-scale Superquadric Fitting for Efficient Shape and Pose Recovery of Unknown Objects." The article in question 7 | can be found here --> http://www.cse.usf.edu/~kkduncan/research/DuncanICRA2013.pdf 8 | 9 | 10 | USAGE 11 | ===== 12 | 13 | See OPEMain.cpp for a demonstration of how to use this code. There are two main functions to use: 14 | 15 | a) calculateObjectPose(pcl::PointCloud\& selectedObjectPtCloud) 16 | 17 | - Found in ObjectPoseEstimator.[h,cpp]. This function is responsible for calculating the pose 18 | of a segmented object represented by the PCL point cloud provided. This function assumes that 19 | the cloud follows the Kinect optical frame. This is then transformed into world coordinates 20 | before superquadric processing. The function returns an instance of the SQParameters class 21 | which possesses the position and orientation of the object. See the comments in SQParameters.h 22 | for an explanation. 23 | 24 | b) run() 25 | 26 | - Also found ObjectPoseEstimator.[h,cpp]. This function assumes that there is a connected Kinect that 27 | faces a table-top of objects within viewing range and that the OpenNI drivers are installed. It 28 | executes as follows: 29 | 30 | 1) Capture a point cloud from the Kinect. 31 | 2) Extract the table-top objects from the captured point cloud. 32 | 3) Present a viewing window to the user in order for them to see the index of the object they 33 | intend to choose. 34 | 4) Asks the user for the index of the object they wish to choose. 35 | 5) Performs pose estimation on the object point cloud the user indicated. 36 | 6) Return the SQParameters instance. 37 | 38 | 39 | NOTE 40 | ==== 41 | 42 | Regular updates to this code would be carried out in the upcoming weeks, so please keep an eye out. There 43 | may be a few bugs in this code as error conditions are not handled as yet. Therefore use at your own 44 | risk. This code is intended for use with Visual Studio 2010 and later. However, this code can be adapted for 45 | any system as long as the following libraries are available: Boost, PCL and all libraries required by PCL (Eigen, VTK, Flann, Qt, QHull, OpenNI), and OpenCV. 46 | 47 | For any questions or comments, please contact kkduncan@cse.usf.edu 48 | 49 | -------------------------------------------------------------------------------- /SQFitting.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | */ 8 | 9 | #include 10 | #include 11 | #include "EigenSolver.h" 12 | #include "OPESettings.h" 13 | #include "SQTypes.h" 14 | #include "SQFitting.h" 15 | 16 | 17 | #ifndef M_PI 18 | #define M_PI 3.141592654 19 | #endif 20 | 21 | 22 | namespace ope { 23 | 24 | SQFitting::SQFitting() { 25 | 26 | } 27 | 28 | 29 | SQFitting::~SQFitting() { 30 | 31 | } 32 | 33 | 34 | void SQFitting::initializeMinimizationParameters (SQParameters& sqParams) { 35 | /// Assign size parameter values based on their bounds properties 36 | if (sqParams.min.a1.type == UNCHANGED) { 37 | sqParams.a1 = sqParams.min.a1.value; 38 | } 39 | if (sqParams.min.a2.type == UNCHANGED) { 40 | sqParams.a2 = sqParams.min.a2.value; 41 | } 42 | if (sqParams.min.a3.type == UNCHANGED) { 43 | sqParams.a3 = sqParams.min.a3.value; 44 | } 45 | 46 | if (sqParams.min.a1.type == BOUNDED) { 47 | sqParams.a1 = (sqParams.min.a1.upperBound + sqParams.min.a1.lowerBound) / 2; 48 | } 49 | if (sqParams.min.a2.type == BOUNDED) { 50 | sqParams.a2 = (sqParams.min.a2.upperBound + sqParams.min.a2.lowerBound) / 2; 51 | } 52 | if (sqParams.min.a3.type == BOUNDED) { 53 | sqParams.a3 = (sqParams.min.a3.upperBound + sqParams.min.a3.lowerBound) / 2; 54 | } 55 | 56 | if (sqParams.min.a1.type == NOT_USED || sqParams.min.a1.type == UNLIMITED) { 57 | sqParams.a1 = 1; 58 | } 59 | if (sqParams.min.a2.type == NOT_USED || sqParams.min.a2.type == UNLIMITED) { 60 | sqParams.a2 = 1; 61 | } 62 | if (sqParams.min.a3.type == NOT_USED || sqParams.min.a3.type == UNLIMITED) { 63 | sqParams.a3 = 1; 64 | } 65 | } 66 | 67 | 68 | 69 | int SQFitting::estimateParameters(const pcl::PointCloud& cloud, SQParameters& sqParams, const int& eigenVector) { 70 | double t[4][4] = {0}; 71 | 72 | // Check if cloud contains enough data points 73 | int len = (int) cloud.points.size(); 74 | if (len < 13) { // changed from 13 75 | return -1; 76 | } 77 | 78 | // Find center of gravity 79 | double a, b, c, x, y, z, xx, yy, zz, cxy, cxz, cyz, count = 0.0; 80 | 81 | x = y = z = 0; 82 | xx = yy = zz = cxy = cyz = cxz = 0; 83 | count = 0; 84 | 85 | for(int i = 0; i < len; i++) { 86 | pcl::PointXYZ pnt = cloud.points[i]; 87 | a = (double) pnt.x; 88 | b = (double) pnt.y; 89 | c = (double) pnt.z; 90 | 91 | x += a; 92 | y += b; 93 | z += c; 94 | 95 | xx += a*a; 96 | yy += b*b; 97 | zz += c*c; 98 | 99 | cxz += a*c; 100 | cyz += b*c; 101 | cxy += a*b; 102 | 103 | count++; 104 | } 105 | 106 | // Compute center of gravity, covariance and variance 107 | double avgx, avgy, avgz; 108 | avgx = x/count; 109 | avgy = y/count; 110 | avgz = z/count; 111 | 112 | xx /= count; 113 | yy /= count; 114 | zz /= count; 115 | 116 | cxz /= count; 117 | cyz /= count; 118 | cxy /= count; 119 | 120 | xx -= (avgx * avgx); 121 | yy -= (avgy * avgy); 122 | zz -= (avgz * avgz); 123 | 124 | cxy -= (avgx * avgy); 125 | cxz -= (avgx * avgz); 126 | cyz -= (avgy * avgz); 127 | 128 | // Build covariance matrix 129 | double aa[3][3]; 130 | aa[0][0] = xx; aa[0][1] = cxy; aa[0][2] = cxz; 131 | aa[1][0] = cxy; aa[1][1] = yy; aa[1][2] = cyz; 132 | aa[2][0] = cxz; aa[2][1] = cyz; aa[2][2] = zz; 133 | 134 | // Compute eigenvectors and eigenvalues 135 | double d[3], m_brain[3][3], m_inverse[3][3]; 136 | double lambdas[3]; 137 | double vectors[3][3]; 138 | int nrot; 139 | 140 | /* 141 | * Using the covariance matrix, we get its eigenvalues 142 | * and eigenvectors 143 | */ 144 | cvJacobiEigens_64d((double*) aa, (double*) m_brain, (double*) d, 3, 0.0); 145 | 146 | // Find the vector with the largest eigenvalue 147 | double max_eigen = -1000000000.0; 148 | int vectorIndex; 149 | 150 | if(d[0] > max_eigen) { 151 | max_eigen = d[0]; 152 | vectorIndex = 0; 153 | } 154 | if(d[1] > max_eigen) { 155 | max_eigen = d[1]; 156 | vectorIndex = 1; 157 | } 158 | if(d[2] > max_eigen) { 159 | max_eigen = d[2]; 160 | vectorIndex = 2; 161 | } 162 | 163 | for (int j = 0; j < 3; j++) { 164 | for (int k = 0; k < 3; k++) { 165 | m_inverse[j][k] = m_brain[j][k]; 166 | } 167 | } 168 | 169 | // Aligning max eigenvector with z axis 170 | #if 0 171 | 172 | for (int j = 0; j < 3; j++) { 173 | m_inverse[j][2] = m_brain[j][eigenVector]; 174 | m_inverse[j][eigenVector] = m_brain[j][2]; 175 | } 176 | 177 | 178 | if (vectorIndex != 2) { 179 | for (int j = 0; j < 3; j++) { 180 | m_inverse[j][vectorIndex] = -m_inverse[j][vectorIndex]; 181 | } 182 | } 183 | 184 | #endif 185 | 186 | for (int j = 0; j < 3; j++) { 187 | for (int k = 0; k < 3; k++) { 188 | m_brain[j][k] = m_inverse[j][k]; 189 | } 190 | } 191 | 192 | // Build transformation matrix (rotation & translation) 193 | for (int i = 0; i < 3; i++) { 194 | for (int j = 0; j < 3; j++){ 195 | t[i][j] = m_brain[i][j]; 196 | } 197 | } 198 | 199 | t[0][3] = avgx; 200 | t[1][3] = avgy; 201 | t[2][3] = avgz; 202 | t[3][3] = 1; 203 | 204 | t[3][0] = t[3][1] = t[3][2] = 0.0; 205 | 206 | // Set / Calculate initial parameter 207 | sqParams.e1 = 1; 208 | sqParams.e2 = 1; 209 | sqParams.kx = 0; 210 | sqParams.ky = 0; 211 | 212 | double xmin, ymin, zmin, xmax, ymax, zmax; 213 | xmin = ymin = zmin = 100000.00; 214 | xmax = ymax = zmax = -100000.00; 215 | 216 | double tInv[4][4]; 217 | matrix_inverse(t, tInv); 218 | 219 | double vector[4]; 220 | for(int i = 0; i < len; i++) { 221 | pcl::PointXYZ pnt = cloud.points[i]; 222 | 223 | vector[0] = (double) pnt.x; 224 | vector[1] = (double) pnt.y; 225 | vector[2] = (double) pnt.z; 226 | vector[3] = 1; 227 | 228 | double result[4]; 229 | matrix_mult(vector, tInv, result); 230 | a = result[0]; 231 | b = result[1]; 232 | c = result[2]; 233 | 234 | if(xmin > a) xmin = a; 235 | if(xmax < a) xmax = a; 236 | 237 | if(ymin > b) ymin = b; 238 | if(ymax < b) ymax = b; 239 | 240 | if(zmin > c) zmin = c; 241 | if(zmax < c) zmax = c; 242 | } 243 | 244 | double r1, r2, r3; 245 | 246 | r1 = atan2(t[1][2], t[0][2]); 247 | r1 = r1 + M_PI; 248 | r2 = atan2(cos(r1) * t[0][2] + sin(r1) * t[1][2], t[2][2]); 249 | r3 = atan2(-sin(r1) * t[0][0] + cos(r1) * t[1][0], -sin(r1) * t[0][1] + cos(r1) * t[1][1]); 250 | 251 | sqParams.phi = r1; 252 | sqParams.theta = r2; 253 | sqParams.psi = r3; 254 | 255 | sqParams.px = t[0][3] + (xmax + xmin) / 2; 256 | sqParams.py = t[1][3] + (ymax + ymin) / 2; 257 | sqParams.pz = t[2][3] + (zmax + zmin) / 2; 258 | 259 | double ta1, ta2, ta3; 260 | 261 | ta1 = (xmax - xmin)/2; 262 | ta2 = (ymax - ymin)/2; 263 | ta3 = (zmax - zmin)/2; 264 | 265 | sqParams.a1 = ta1; 266 | sqParams.a2 = ta2; 267 | sqParams.a3 = ta3; 268 | 269 | return 0; 270 | } 271 | 272 | 273 | 274 | int SQFitting::estimateParametersNew(const pcl::PointCloud& cloud, SQParameters& sqParams, const int& eigenVector) { 275 | double t[4][4] = {0}; 276 | 277 | // Check if cloud contains enough data points 278 | int len = (int) cloud.points.size(); 279 | if (len < 13) { // changed from 13 280 | return -1; 281 | } 282 | 283 | // Find center of gravity 284 | double a, b, c, x, y, z, xx, yy, zz, cxy, cxz, cyz, count; 285 | 286 | x = y = z = 0; 287 | xx = yy = zz = cxy = cyz = cxz = 0; 288 | count = 0; 289 | 290 | for(int i = 0; i < len; i++) { 291 | pcl::PointXYZ pnt = cloud.points[i]; 292 | a = (double) pnt.x; 293 | b = (double) pnt.y; 294 | c = (double) pnt.z; 295 | 296 | x += a; 297 | y += b; 298 | z += c; 299 | 300 | xx += a*a; 301 | yy += b*b; 302 | zz += c*c; 303 | 304 | cxz += a*c; 305 | cyz += b*c; 306 | cxy += a*b; 307 | 308 | count++; 309 | } 310 | 311 | // Compute center of gravity, covariance and variance 312 | double avgx, avgy, avgz; 313 | avgx = x/count; 314 | avgy = y/count; 315 | avgz = z/count; 316 | 317 | xx /= count; 318 | yy /= count; 319 | zz /= count; 320 | 321 | cxz /= count; 322 | cyz /= count; 323 | cxy /= count; 324 | 325 | xx -= (avgx * avgx); 326 | yy -= (avgy * avgy); 327 | zz -= (avgz * avgz); 328 | 329 | cxy -= (avgx * avgy); 330 | cxz -= (avgx * avgz); 331 | cyz -= (avgy * avgz); 332 | 333 | // Build covariance matrix 334 | double aa[3][3]; 335 | aa[0][0] = xx; aa[0][1] = cxy; aa[0][2] = cxz; 336 | aa[1][0] = cxy; aa[1][1] = yy; aa[1][2] = cyz; 337 | aa[2][0] = cxz; aa[2][1] = cyz; aa[2][2] = zz; 338 | 339 | // Compute eigenvectors and eigenvalues 340 | double d[3], m_brain[3][3], m_inverse[3][3]; 341 | double lambdas[3], vectors[3][3]; 342 | int nrot; 343 | 344 | /* 345 | * Using the covariance matrix, we get its eigenvalues 346 | * and eigenvectors 347 | */ 348 | cvJacobiEigens_64d((double*) aa, (double*) m_brain, (double*) d, 3, 0.0); 349 | 350 | // Find the vector with the largest eigenvalue 351 | double max_eigen = -1000000000.0; 352 | int vectorIndex; 353 | int longestAxis; 354 | int longestAxisLambda; 355 | 356 | #if 0 357 | if (abs(lambdas[2] - lambdas[1]) < abs(lambdas[1] - lambdas[0])) { 358 | longestAxisLambda = 0; 359 | } else { 360 | longestAxisLambda = 2; 361 | } 362 | 363 | for (int i = 0; i < 3; i++) { 364 | if (vectors[0][longestAxisLambda] == m_brain[0][i] && 365 | vectors[1][longestAxisLambda] == m_brain[1][i] && 366 | vectors[2][longestAxisLambda] == m_brain[2][i]) { 367 | vectorIndex = i; 368 | } 369 | } 370 | 371 | #endif 372 | 373 | if(d[0] > max_eigen) { 374 | max_eigen = d[0]; 375 | vectorIndex = 0; 376 | } 377 | if(d[1] > max_eigen) { 378 | max_eigen = d[1]; 379 | vectorIndex = 1; 380 | } 381 | if(d[2] > max_eigen) { 382 | max_eigen = d[2]; 383 | vectorIndex = 2; 384 | } 385 | 386 | for (int j = 0; j < 3; j++) { 387 | for (int k = 0; k < 3; k++) { 388 | m_inverse[j][k] = m_brain[j][k]; 389 | } 390 | } 391 | 392 | // Aligning max eigenvector with z axis 393 | #if 0 394 | for (int j = 0; j < 3; j++) { 395 | m_inverse[j][2] = m_brain[j][eigenVector]; 396 | m_inverse[j][eigenVector] = m_brain[j][2]; 397 | } 398 | 399 | if (vectorIndex != 2) { 400 | for (int j = 0; j < 3; j++) { 401 | m_inverse[j][vectorIndex] = -m_inverse[j][vectorIndex]; 402 | } 403 | } 404 | 405 | for (int j = 0; j < 3; j++) { 406 | for (int k = 0; k < 3; k++) { 407 | m_brain[j][k] = m_inverse[j][k]; 408 | } 409 | } 410 | 411 | #endif 412 | 413 | // Build transformation matrix (rotation & translation) 414 | for (int i = 0; i < 3; i++) { 415 | for (int j = 0; j < 3; j++){ 416 | t[i][j] = m_brain[i][j]; 417 | } 418 | } 419 | 420 | t[0][3] = avgx; 421 | t[1][3] = avgy; 422 | t[2][3] = avgz; 423 | t[3][3] = 1; 424 | 425 | t[3][0] = t[3][1] = t[3][2] = 0.0; 426 | 427 | // Set / Calculate initial parameter 428 | sqParams.e1 = 1; 429 | sqParams.e2 = 1; 430 | sqParams.kx = 0; 431 | sqParams.ky = 0; 432 | 433 | double xmin, ymin, zmin, xmax, ymax, zmax; 434 | xmin = ymin = zmin = 100000.00; 435 | xmax = ymax = zmax = -100000.00; 436 | 437 | double tInv[4][4]; 438 | matrix_inverse(t, tInv); 439 | 440 | double vector[4]; 441 | for(int i = 0; i < len; i++) { 442 | pcl::PointXYZ pnt = cloud.points[i]; 443 | 444 | vector[0] = (double) pnt.x; 445 | vector[1] = (double) pnt.y; 446 | vector[2] = (double) pnt.z; 447 | vector[3] = 1; 448 | 449 | double result[4]; 450 | matrix_mult(vector, tInv, result); 451 | a = result[0]; 452 | b = result[1]; 453 | c = result[2]; 454 | 455 | if(xmin > a) xmin = a; 456 | if(xmax < a) xmax = a; 457 | 458 | if(ymin > b) ymin = b; 459 | if(ymax < b) ymax = b; 460 | 461 | if(zmin > c) zmin = c; 462 | if(zmax < c) zmax = c; 463 | } 464 | 465 | double r1, r2, r3; 466 | 467 | r1 = atan2(t[1][2], t[0][2]); 468 | r1 = r1 + M_PI; 469 | r2 = atan2(cos(r1) * t[0][2] + sin(r1) * t[1][2], t[2][2]); 470 | r3 = atan2(-sin(r1) * t[0][0] + cos(r1) * t[1][0], -sin(r1) * t[0][1] + cos(r1) * t[1][1]); 471 | 472 | sqParams.phi = r1; 473 | sqParams.theta = r2; 474 | sqParams.psi = r3; 475 | 476 | sqParams.px = t[0][3] + (xmax + xmin) / 2; 477 | sqParams.py = t[1][3] + (ymax + ymin) / 2; 478 | sqParams.pz = t[2][3] + (zmax + zmin) / 2; 479 | 480 | double ta1, ta2, ta3; 481 | 482 | ta1 = (xmax - xmin)/2; 483 | ta2 = (ymax - ymin)/2; 484 | ta3 = (zmax - zmin)/2; 485 | 486 | sqParams.a1 = ta1; 487 | sqParams.a2 = ta2; 488 | sqParams.a3 = ta3; 489 | 490 | return vectorIndex; 491 | 492 | } 493 | 494 | 495 | 496 | void SQFitting::estimateInitialParameters(pcl::PointCloud& cloud, SQParameters& sqParams) { 497 | double minError = 1000000; 498 | int maxEigenVector = 0; 499 | int retVal; 500 | 501 | for (int i = 0; i < 3; i++) { 502 | retVal = estimateParameters(cloud, sqParams, i); 503 | if (retVal == -1) { 504 | printf("Error: At least 13 data points must support the fit!\n"); 505 | } 506 | 507 | double error = 0; 508 | error = minProcess.errorFunc(cloud, sqParams); 509 | 510 | if (error < minError) { 511 | minError = error; 512 | maxEigenVector = i; 513 | } 514 | } 515 | 516 | retVal = estimateParameters(cloud, sqParams, maxEigenVector); 517 | } 518 | 519 | 520 | double SQFitting::qualityOfFit(pcl::PointCloud& cloud, SQParameters& sqParams) { 521 | pcl::PointXYZ _pnt; 522 | 523 | Eigen::MatrixXd m(3, 3), _m(3, 3); 524 | 525 | m(0, 0) = cos(sqParams.phi) * cos(sqParams.theta) * cos(sqParams.psi) - sin(sqParams.phi) * sin(sqParams.psi); 526 | m(0, 1) = -cos(sqParams.phi) * cos(sqParams.theta) * sin(sqParams.psi) - sin(sqParams.phi) * cos(sqParams.psi); 527 | m(0, 2) = cos(sqParams.phi) * sin(sqParams.theta); 528 | m(1, 0) = sin(sqParams.phi) * cos(sqParams.theta) * cos(sqParams.psi) + cos(sqParams.phi) * sin(sqParams.psi); 529 | m(1, 1) = -sin(sqParams.phi) * cos(sqParams.theta) * sin(sqParams.psi) + cos(sqParams.phi) * cos(sqParams.psi); 530 | m(1, 2) = sin(sqParams.phi) * sin(sqParams.theta); 531 | m(2, 0) = -sin(sqParams.theta) * cos(sqParams.psi); 532 | m(2, 1) = sin(sqParams.theta) * sin(sqParams.psi); 533 | m(2, 2) = cos(sqParams.theta); 534 | 535 | _m = m.inverse(); 536 | 537 | double f, a, b, c, d, e, error = 0; 538 | 539 | for (int i = 0; i < (int) cloud.points.size(); i++) { 540 | Eigen::MatrixXd pntMove(3, 1); 541 | Eigen::MatrixXd pnt(3, 1); 542 | Eigen::MatrixXd _pnt(3, 1); 543 | 544 | pntMove(0, 0) = (double) cloud.points[i].x - sqParams.px; 545 | pntMove(1, 0) = (double) cloud.points[i].y - sqParams.py; 546 | pntMove(2, 0) = (double) cloud.points[i].z - sqParams.pz; 547 | 548 | _pnt = _m * pntMove; 549 | 550 | _pnt(0, 0) = _pnt(0, 0) / (sqParams.kx * _pnt(2, 0) / sqParams.a3 + 1); 551 | _pnt(1, 0) = _pnt(1, 0) / (sqParams.ky * _pnt(2, 0) / sqParams.a3 + 1); 552 | 553 | a = pow(fabs(_pnt(0, 0) / sqParams.a1), 2.0 / sqParams.e2); 554 | b = pow(fabs(_pnt(1, 0) / sqParams.a2), 2.0 / sqParams.e2); 555 | c = pow(fabs(_pnt(2, 0) / sqParams.a3), 2.0 / sqParams.e1); 556 | d = pow(a + b, (double) (sqParams.e2 / sqParams.e1)); 557 | e = d + c; 558 | f = pow(e, (double) (sqParams.e1)); 559 | 560 | double tempError = pow((f - 1), 2.0); 561 | error += exp(-tempError * 10000); 562 | 563 | } 564 | 565 | error = sqrt(error); 566 | 567 | return (error); 568 | } 569 | 570 | 571 | double SQFitting::recoverParameters(const pcl::PointCloud& cloud, SQParameters& prm) { 572 | int gliset; 573 | double gloldm; 574 | 575 | int npt, mfit, n_model_acc, iter; 576 | double alamda, old_chisq; 577 | 578 | glmma a; 579 | glndata F; 580 | glndata sig; 581 | glndata2 xw; 582 | gllista lista; 583 | glcovar covar, alpha; 584 | 585 | gliset = 0; 586 | gloldm = -1.0; 587 | minProcess.i_am_in_trouble = 0; 588 | 589 | a[0] = prm.a1; 590 | a[1] = prm.a2; 591 | a[2] = prm.a3; 592 | a[3] = prm.e1; 593 | a[4] = prm.e2; 594 | a[5] = prm.phi; 595 | a[6] = prm.theta; 596 | a[7] = prm.psi; 597 | a[8] = prm.px; 598 | a[9] = prm.py; 599 | a[10] = prm.pz; 600 | a[11] = prm.kx; 601 | a[12] = prm.ky; 602 | a[13] = 0; /* center of bending */ 603 | a[14] = -500; /* z min */ 604 | a[15] = 500; /* z max */ 605 | a[16] = 0.0000010; /* k or 1/k ?? */ 606 | a[17] = 0; /* alpha angle */ 607 | 608 | for (int i = 0; i < MAX_PARAMS; i++) lista[i] = i; 609 | 610 | mfit = 13; 611 | int index = 0; 612 | 613 | if (prm.min.a1.type == NOT_USED || prm.min.a1.type == UNCHANGED) mfit--; 614 | else { 615 | lista[index] = 0; 616 | index++; 617 | } 618 | if (prm.min.a1.type == UNCHANGED) a[0] = prm.min.a1.value; 619 | else if (prm.min.a1.type == BOUNDED) a[0] = (prm.min.a1.upperBound + prm.min.a1.lowerBound) / 2; 620 | 621 | if (prm.min.a2.type == NOT_USED || prm.min.a2.type == UNCHANGED) mfit--; 622 | else { 623 | lista[index] = 1; 624 | index++; 625 | } 626 | if (prm.min.a2.type == UNCHANGED) a[1] = prm.min.a2.value; 627 | else if (prm.min.a2.type == BOUNDED) a[1] = (prm.min.a2.upperBound + prm.min.a2.lowerBound) / 2; 628 | 629 | if (prm.min.a3.type == NOT_USED || prm.min.a3.type == UNCHANGED) mfit--; 630 | else { 631 | lista[index] = 2; 632 | index++; 633 | } 634 | if (prm.min.a3.type == UNCHANGED) a[2] = prm.min.a3.value; 635 | else if (prm.min.a3.type == BOUNDED) a[2] = (prm.min.a3.upperBound + prm.min.a3.lowerBound) / 2; 636 | 637 | if (prm.min.e1.type == NOT_USED || prm.min.e1.type == UNCHANGED) mfit--; 638 | else { 639 | lista[index] = 3; 640 | index++; 641 | } 642 | if (prm.min.e1.type == UNCHANGED) a[3] = prm.min.e1.value; 643 | else if (prm.min.e1.type == BOUNDED) a[3] = (prm.min.e1.upperBound + prm.min.e1.lowerBound) / 2; 644 | 645 | if (prm.min.e2.type == NOT_USED || prm.min.e2.type == UNCHANGED) mfit--; 646 | else { 647 | lista[index] = 4; 648 | index++; 649 | } 650 | if (prm.min.e2.type == UNCHANGED) a[4] = prm.min.e2.value; 651 | else if (prm.min.e2.type == BOUNDED) a[4] = (prm.min.e2.upperBound + prm.min.e2.lowerBound) / 2; 652 | 653 | if (prm.min.phi.type == NOT_USED || prm.min.phi.type == UNCHANGED) mfit--; 654 | else { 655 | lista[index] = 5; 656 | index++; 657 | } 658 | if (prm.min.phi.type == UNCHANGED) a[5] = prm.min.phi.value; 659 | else if (prm.min.phi.type == BOUNDED) a[5] = (prm.min.phi.upperBound + prm.min.phi.lowerBound) / 2; 660 | 661 | if (prm.min.theta.type == NOT_USED || prm.min.theta.type == UNCHANGED) mfit--; 662 | else { 663 | lista[index] = 6; 664 | index++; 665 | } 666 | if (prm.min.theta.type == UNCHANGED) a[6] = prm.min.theta.value; 667 | else if (prm.min.phi.type == BOUNDED) a[6] = (prm.min.theta.upperBound + prm.min.theta.lowerBound) / 2; 668 | 669 | if (prm.min.psi.type == NOT_USED || prm.min.psi.type == UNCHANGED) mfit--; 670 | else { 671 | lista[index] = 7; 672 | index++; 673 | } 674 | if (prm.min.psi.type == UNCHANGED) a[7] = prm.min.psi.value; 675 | else if (prm.min.psi.type == BOUNDED) a[7] = (prm.min.psi.upperBound + prm.min.psi.lowerBound) / 2; 676 | 677 | if (prm.min.px.type == NOT_USED || prm.min.px.type == UNCHANGED) mfit--; 678 | else { 679 | lista[index] = 8; 680 | index++; 681 | } 682 | if (prm.min.px.type == UNCHANGED) a[8] = prm.min.px.value; 683 | else if (prm.min.px.type == BOUNDED) a[8] = (prm.min.px.upperBound + prm.min.px.lowerBound) / 2; 684 | 685 | if (prm.min.py.type == NOT_USED || prm.min.py.type == UNCHANGED) mfit--; 686 | else { 687 | lista[index] = 9; 688 | index++; 689 | } 690 | if (prm.min.py.type == UNCHANGED) a[9] = prm.min.py.value; 691 | else if (prm.min.py.type == BOUNDED) a[9] = (prm.min.py.upperBound + prm.min.py.lowerBound) / 2; 692 | 693 | if (prm.min.pz.type == NOT_USED || prm.min.pz.type == UNCHANGED) mfit--; 694 | else { 695 | lista[index] = 10; 696 | index++; 697 | } 698 | if (prm.min.pz.type == UNCHANGED) a[10] = prm.min.pz.value; 699 | else if (prm.min.pz.type == BOUNDED) a[10] = (prm.min.pz.upperBound + prm.min.pz.lowerBound) / 2; 700 | 701 | if (prm.min.kx.type == NOT_USED || prm.min.kx.type == UNCHANGED) mfit--; 702 | else { 703 | lista[index] = 11; 704 | index++; 705 | } 706 | if (prm.min.kx.type == UNCHANGED) a[11] = prm.min.kx.value; 707 | else if (prm.min.kx.type == BOUNDED) a[11] = (prm.min.kx.upperBound + prm.min.kx.lowerBound) / 2; 708 | 709 | if (prm.min.ky.type == NOT_USED || prm.min.ky.type == UNCHANGED) mfit--; 710 | else { 711 | lista[index] = 12; 712 | index++; 713 | } 714 | if (prm.min.ky.type == UNCHANGED) prm.ky = prm.min.ky.value; 715 | else if (prm.min.ky.type == BOUNDED) a[12] = (prm.min.ky.upperBound + prm.min.ky.lowerBound) / 2; 716 | 717 | // Prepare surface points 718 | npt = (int) cloud.points.size(); 719 | 720 | if (npt > MAX_POINTS) { 721 | npt = MAX_POINTS; 722 | } 723 | 724 | for (int i = 0; i < npt; i++) { 725 | pcl::PointXYZ pt = cloud.points[i]; 726 | ope::MPoint p; 727 | p[0] = (double) pt.x; 728 | p[1] = (double) pt.y; 729 | p[2] = (double) pt.z; 730 | xw.push_back(p); 731 | F.push_back(0); 732 | sig.push_back(1); 733 | 734 | } 735 | 736 | n_model_acc = npt; 737 | minProcess.glochisq = 10e31; 738 | old_chisq = 1.0; 739 | iter = 0; 740 | 741 | for (int lmIter = 0; lmIter < prm.min.iterations; lmIter++) { 742 | if (lmIter == 0) 743 | alamda = minProcess.mrqmin_init(xw, F, sig, npt, a, lista, mfit, alpha, MAX_PARAMS, &n_model_acc); 744 | else 745 | alamda = minProcess.mrqmin(prm, xw, F, sig, npt, a, lista, mfit, covar, alpha, alamda, &n_model_acc); 746 | 747 | if (minProcess.i_am_in_trouble) { 748 | return(-1); 749 | } 750 | 751 | if(old_chisq != minProcess.glochisq) { 752 | old_chisq = minProcess.glochisq; 753 | iter = iter + 1; 754 | } 755 | 756 | } 757 | 758 | ///////// Set Return Values //////// 759 | // Size Parameters 760 | if (prm.min.a1.type != NOT_USED && prm.min.a1.type != UNCHANGED) prm.a1 = a[0]; 761 | else if (prm.min.a1.type == BOUNDED) prm.a1 = a[0]; 762 | else prm.a1 = prm.min.a1.value; 763 | 764 | if (prm.min.a2.type != NOT_USED && prm.min.a2.type != UNCHANGED) prm.a2 = a[1]; 765 | else if (prm.min.a2.type == BOUNDED) prm.a2 = a[1]; 766 | else prm.a2 = prm.min.a2.value; 767 | 768 | if (prm.min.a3.type != NOT_USED && prm.min.a3.type != UNCHANGED) prm.a3 = a[2]; 769 | else if (prm.min.a3.type == BOUNDED) prm.a3 = a[2]; 770 | else prm.a3 = prm.min.a3.value; 771 | 772 | 773 | // Shape Parameters 774 | if (prm.min.e1.type != NOT_USED && prm.min.e1.type != UNCHANGED) prm.e1 = a[3]; 775 | else if (prm.min.e1.type == BOUNDED) prm.e1 = a[3]; 776 | else prm.e1 = prm.min.e1.value; 777 | 778 | if (prm.min.e2.type != NOT_USED && prm.min.e2.type != UNCHANGED) prm.e2 = a[4]; 779 | else if (prm.min.e2.type == BOUNDED) prm.e2 = a[4]; 780 | else prm.e2 = prm.min.e2.value; 781 | 782 | 783 | // Rotation Parameters 784 | if (prm.min.phi.type != NOT_USED && prm.min.phi.type != UNCHANGED) prm.phi = a[5]; 785 | else if (prm.min.phi.type == BOUNDED) prm.phi = a[5]; 786 | else prm.phi = prm.min.phi.value; 787 | 788 | if (prm.min.theta.type != NOT_USED && prm.min.theta.type != UNCHANGED) prm.theta = a[6]; 789 | else if (prm.min.theta.type == BOUNDED) prm.theta = a[6]; 790 | else prm.theta = prm.min.theta.value; 791 | 792 | if (prm.min.psi.type != NOT_USED && prm.min.psi.type != UNCHANGED) prm.psi = a[7]; 793 | else if (prm.min.psi.type == BOUNDED) prm.psi = a[7]; 794 | else prm.psi = prm.min.psi.value; 795 | 796 | 797 | // Position (translation) Parameters 798 | if (prm.min.px.type != NOT_USED && prm.min.px.type != UNCHANGED) prm.px = a[8]; 799 | else if (prm.min.px.type == BOUNDED) prm.px = a[8]; 800 | else prm.px = prm.min.px.value; 801 | 802 | if (prm.min.py.type != NOT_USED && prm.min.py.type != UNCHANGED) prm.py = a[9]; 803 | else if (prm.min.py.type == BOUNDED) prm.py = a[9]; 804 | else prm.py = prm.min.py.value; 805 | 806 | if (prm.min.pz.type != NOT_USED && prm.min.pz.type != UNCHANGED) prm.pz = a[10]; 807 | else if (prm.min.pz.type == BOUNDED) prm.pz = a[10]; 808 | else prm.pz = prm.min.pz.value; 809 | 810 | 811 | // Taper Parameters 812 | if (prm.min.kx.type != NOT_USED && prm.min.kx.type != UNCHANGED) prm.kx = a[11]; 813 | else if (prm.min.kx.type == BOUNDED) prm.kx = a[11]; 814 | else prm.kx = prm.min.kx.value; 815 | 816 | if (prm.min.ky.type != NOT_USED && prm.min.ky.type != UNCHANGED) prm.ky = a[12]; 817 | else if (prm.min.ky.type == BOUNDED) prm.ky = a[12]; 818 | else prm.ky = prm.min.ky.value; 819 | 820 | double error = 0; 821 | error = minProcess.errorFunc(cloud, prm); 822 | 823 | return (error); 824 | } 825 | 826 | 827 | 828 | void SQFitting::performShapeFitting(const pcl::PointCloud& cloud, SQParameters& initParams, SQParameters& bestParams) { 829 | pcl::PointCloud tempSQCloud; 830 | SQParameters unrefinedParams, refinedParams; 831 | double error, minError = 1.0e13;; 832 | int ret = -1, eigenVect; 833 | int iterations; 834 | 835 | // Get best eigenvector 836 | iterations = initParams.min.iterations; 837 | initParams.min.iterations = 20; 838 | 839 | for (int i = 0; i < 3; i++) { 840 | ret = estimateParameters(cloud, initParams, i); 841 | if (ret == -1) { 842 | printf("Error: Cannot perform superquadric fitting!\n"); 843 | } 844 | 845 | error = recoverParameters(cloud, initParams); 846 | if (error < minError) { 847 | minError = error; 848 | eigenVect = i; 849 | } 850 | } 851 | 852 | initParams.min.iterations = iterations; 853 | initParams.principalAxis = eigenVect; 854 | 855 | ret = estimateParameters(cloud, initParams, eigenVect); 856 | if (ret == -1) { 857 | printf("Error: Cannot perform superquadric fitting!\n"); 858 | error = 100000; 859 | } else { 860 | error = recoverParameters(cloud, initParams); 861 | } 862 | 863 | initParams.copyTo(bestParams); 864 | } 865 | 866 | 867 | } // ope 868 | 869 | 870 | -------------------------------------------------------------------------------- /SQFitting.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file SQFitting.h 34 | * \brief Performs superquadric fitting on a point cloud 35 | * \author Kester Duncan 36 | * \note Adapted from G. Biegelbauer's implementation 37 | * \details Defines all the functions necessary for Superquadric fitting 38 | */ 39 | #pragma once 40 | #ifndef SQ_FITTING_H__ 41 | #define SQ_FITTING_H__ 42 | 43 | 44 | #include "Minimization.h" 45 | 46 | 47 | 48 | ////////////// Forward declarations to avoid large headers //////////////// 49 | namespace pcl { 50 | 51 | class PointXYZ; 52 | template class PointCloud; 53 | 54 | } // pcl 55 | 56 | 57 | class SQParameters; 58 | 59 | 60 | namespace ope { 61 | 62 | 63 | /** 64 | * \brief Fits a parametric superquadric to a point cloud 65 | */ 66 | class SQFitting { 67 | private: 68 | /** 69 | * \brief Object to execute all minimization functions 70 | */ 71 | Minimization minProcess; 72 | 73 | 74 | public: 75 | /// Constructor 76 | SQFitting(); 77 | 78 | /// Destructor 79 | ~SQFitting(); 80 | 81 | 82 | /** 83 | * \brief Initializes the Levenberg-Marquadt minimization parameters used for sq estimation 84 | */ 85 | void initializeMinimizationParameters (SQParameters& sqParams); 86 | 87 | 88 | /** 89 | * \brief Estimate the parameters of a superquadric from the given point cloud 90 | * \return the eigen vector index with the largest variance 91 | */ 92 | int estimateParameters(const pcl::PointCloud& cloud, SQParameters& sqParams, const int& eigenVector); 93 | 94 | 95 | /** 96 | * \brief Estimate the parameters of a superquadric from the given point cloud 97 | * \return the eigen vector index with the largest variance 98 | */ 99 | int estimateParametersNew(const pcl::PointCloud& cloud, SQParameters& sqParams, const int& eigenVector); 100 | 101 | 102 | /** 103 | * \brief Estimate the initial SQ parameters based on eigen analysis 104 | */ 105 | void estimateInitialParameters(pcl::PointCloud& cloud, SQParameters& sqParams); 106 | 107 | 108 | /** 109 | * \brief Determines the total error (quality) of fit of superquadric parameters 110 | */ 111 | double qualityOfFit(pcl::PointCloud& cloud, SQParameters& sqParams); 112 | 113 | 114 | /** 115 | * \brief Recover the parameters of the superquadric that best fits the given point cloud 116 | */ 117 | double recoverParameters(const pcl::PointCloud& cloud, SQParameters& prm); 118 | 119 | 120 | /** 121 | * \brief Executes the superquadric shape fitting process on a point cloud and gets the best parameters 122 | * \param the initial superquadric parameters for the given cloud 123 | * \param the final superquadric parameters recovered after processing 124 | */ 125 | void performShapeFitting(const pcl::PointCloud& cloud, SQParameters& initParams, SQParameters& bestParams); 126 | 127 | 128 | }; 129 | 130 | 131 | } // ope 132 | 133 | #endif /* SQ_FITTING_H__ */ 134 | -------------------------------------------------------------------------------- /SQTypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file SQTypes.h 34 | * \brief Defines types used for Superquadric shape fitting 35 | * \author Kester Duncan 36 | */ 37 | #pragma once 38 | #ifndef __SHAPEFITTINGTYPES_H__ 39 | #define __SHAPEFITTINGTYPES_H__ 40 | 41 | #include 42 | #include "OPESettings.h" 43 | 44 | /** \namespace ope 45 | * Namespace where all the Object Pose Estimation functionality resides 46 | */ 47 | namespace ope { 48 | 49 | /// Defines the numerical property of a superquadric parameter 50 | typedef enum ParameterType { 51 | UNCHANGED = 1, 52 | BOUNDED, 53 | UNLIMITED, 54 | NOT_USED, 55 | OFFSET 56 | 57 | } ParameterType; 58 | 59 | 60 | /// Defines the upper and lower limits for superquadric parameters 61 | typedef struct ParameterLimits { 62 | ParameterType type; 63 | double value; 64 | double lowerBound; 65 | double upperBound; 66 | ParameterLimits() : value(0.f), lowerBound(0.f), upperBound(0.f) {} 67 | 68 | } ParameterLimits; 69 | 70 | 71 | /// Defines the minimization parameters that would be used for minimization during SQ fitting 72 | typedef struct MinimizationParameters { 73 | int iterations; 74 | ParameterLimits a1; 75 | ParameterLimits a2; 76 | ParameterLimits a3; 77 | 78 | ParameterLimits e1; 79 | ParameterLimits e2; 80 | 81 | ParameterLimits phi; 82 | ParameterLimits theta; 83 | ParameterLimits psi; 84 | 85 | ParameterLimits px; 86 | ParameterLimits py; 87 | ParameterLimits pz; 88 | 89 | ParameterLimits kx; 90 | ParameterLimits ky; 91 | 92 | MinimizationParameters() : iterations(20) {} 93 | 94 | } MinimizationParameters; 95 | 96 | 97 | /** 98 | * \brief Defines the Superquadric parameters used for object pose estimation 99 | * 100 | */ 101 | class SQParameters{ 102 | public: 103 | /// The shape dimension for the x-axis 104 | double a1; 105 | /// The shape dimension for the y-axis 106 | double a2; 107 | /// The shape dimension for the z-axis 108 | double a3; 109 | 110 | /// The north-south superquadric shape parameter 111 | double e1; 112 | /// The east-west superquadric shape parameter 113 | double e2; 114 | 115 | /// The x-axis location of the centroid of this superquadric 116 | double px; 117 | /// The y-axis location of the centroid of this superquadric 118 | double py; 119 | /// The z-axis location of the centroid of this superquadric 120 | double pz; 121 | 122 | /// Euler rotation angle along the x-axis 123 | double phi; 124 | /// Euler rotation angle along the y-axis 125 | double theta; 126 | /// Euler rotation angle along the x-axis 127 | double psi; 128 | 129 | /// Tapering parameter along the x-axis 130 | double kx; 131 | /// Tapering parameter along the y-axis 132 | double ky; 133 | 134 | /// Index of the principal axis in the calculated rotation matrix 135 | int principalAxis; 136 | int majorAxis; 137 | int minorAxis; 138 | 139 | /// Minimization properties 140 | MinimizationParameters min; 141 | 142 | 143 | /// Default Constructor 144 | SQParameters() { 145 | a1 = 0.0; a2 = 0.0; a3 = 0.0; 146 | e1 = 1.0; e2 = 1.0; 147 | px = 0.0; py = 0.0; pz = 0.0; 148 | phi = 0.0; theta = 0.0; psi = 0.0; 149 | kx = 0.0; ky = 0.0; 150 | principalAxis = 0; 151 | majorAxis = 0; 152 | minorAxis = 0; 153 | 154 | min.iterations = 20; 155 | min.a1.type = BOUNDED; 156 | min.a1.lowerBound = 0.020; 157 | min.a1.upperBound = 0.30; 158 | a1 = 0.05; 159 | 160 | min.a2.type = BOUNDED; 161 | min.a2.lowerBound = 0.020; 162 | min.a2.upperBound = 0.30; 163 | a2 = 0.05; 164 | 165 | min.a3.type = BOUNDED; 166 | min.a3.lowerBound = 0.020; 167 | min.a3.upperBound = 0.30; 168 | a3 = 0.05; 169 | 170 | min.e1.type = BOUNDED; 171 | min.e1.lowerBound = 0.1; 172 | min.e1.upperBound = 2.0; 173 | e1 = 1; 174 | 175 | min.e2.type = BOUNDED; 176 | min.e2.lowerBound = 0.1f; 177 | min.e2.upperBound = 2.0f; 178 | e2 = 1; 179 | 180 | min.phi.type = UNLIMITED; 181 | min.theta.type = UNLIMITED; 182 | min.psi.type = UNLIMITED; 183 | min.phi.value = 1.0; 184 | min.theta.value = 1.0; 185 | min.psi.value = 1.0; 186 | 187 | min.px.type = UNLIMITED; 188 | min.py.type = UNLIMITED; 189 | min.pz.type = UNLIMITED; 190 | 191 | min.kx.type = BOUNDED; 192 | min.kx.lowerBound = -1.0; 193 | min.kx.upperBound = 1.0; 194 | min.kx.value = 0.0; 195 | kx = 1.0; 196 | 197 | min.ky.type = BOUNDED; 198 | min.ky.lowerBound = -1.0; 199 | min.ky.upperBound = 1.0; 200 | min.ky.value = 0.0; 201 | ky = 1.0; 202 | 203 | 204 | 205 | } 206 | 207 | 208 | /// Deep copy of this class' properties 209 | void copyTo(SQParameters& sqParams) { 210 | sqParams.a1 = this->a1; 211 | sqParams.a2 = this->a2; 212 | sqParams.a3 = this->a3; 213 | sqParams.e1 = this->e1; 214 | sqParams.e2 = this->e2; 215 | sqParams.psi = this->psi; 216 | sqParams.theta = this->theta; 217 | sqParams.phi = this->phi; 218 | sqParams.px = this->px; 219 | sqParams.py = this->py; 220 | sqParams.pz = this->pz; 221 | sqParams.kx = this->kx; 222 | sqParams.ky = this->ky; 223 | sqParams.principalAxis = this->principalAxis; 224 | sqParams.majorAxis = this->majorAxis; 225 | sqParams.minorAxis = this->minorAxis; 226 | 227 | sqParams.min.a1.lowerBound = this->min.a1.lowerBound; 228 | sqParams.min.a1.upperBound = this->min.a1.upperBound; 229 | sqParams.min.a1.type = this->min.a1.type; 230 | sqParams.min.a1.value = this->min.a1.value; 231 | 232 | sqParams.min.a2.lowerBound = this->min.a2.lowerBound; 233 | sqParams.min.a2.upperBound = this->min.a2.upperBound; 234 | sqParams.min.a2.type = this->min.a2.type; 235 | sqParams.min.a2.value = this->min.a2.value; 236 | 237 | sqParams.min.a3.lowerBound = this->min.a3.lowerBound; 238 | sqParams.min.a3.upperBound = this->min.a3.upperBound; 239 | sqParams.min.a3.type = this->min.a3.type; 240 | sqParams.min.a3.value = this->min.a3.value; 241 | 242 | sqParams.min.e1.lowerBound = this->min.e1.lowerBound; 243 | sqParams.min.e1.upperBound = this->min.e1.upperBound; 244 | sqParams.min.e1.type = this->min.e1.type; 245 | sqParams.min.e1.value = this->min.e1.value; 246 | 247 | sqParams.min.e2.lowerBound = this->min.e2.lowerBound; 248 | sqParams.min.e2.upperBound = this->min.e2.upperBound; 249 | sqParams.min.e2.type = this->min.e2.type; 250 | sqParams.min.e2.value = this->min.e2.value; 251 | 252 | sqParams.min.psi.lowerBound = this->min.psi.lowerBound; 253 | sqParams.min.psi.upperBound = this->min.psi.upperBound; 254 | sqParams.min.psi.type = this->min.psi.type; 255 | sqParams.min.psi.value = this->min.psi.value; 256 | 257 | sqParams.min.theta.lowerBound = this->min.theta.lowerBound; 258 | sqParams.min.theta.upperBound = this->min.theta.upperBound; 259 | sqParams.min.theta.type = this->min.theta.type; 260 | sqParams.min.theta.value = this->min.theta.value; 261 | 262 | sqParams.min.phi.lowerBound = this->min.phi.lowerBound; 263 | sqParams.min.phi.upperBound = this->min.phi.upperBound; 264 | sqParams.min.phi.type = this->min.phi.type; 265 | sqParams.min.phi.value = this->min.phi.value; 266 | 267 | sqParams.min.px.lowerBound = this->min.px.lowerBound; 268 | sqParams.min.px.upperBound = this->min.px.upperBound; 269 | sqParams.min.px.type = this->min.px.type; 270 | sqParams.min.px.value = this->min.px.value; 271 | 272 | sqParams.min.py.lowerBound = this->min.py.lowerBound; 273 | sqParams.min.py.upperBound = this->min.py.upperBound; 274 | sqParams.min.py.type = this->min.py.type; 275 | sqParams.min.py.value = this->min.py.value; 276 | 277 | sqParams.min.pz.lowerBound = this->min.pz.lowerBound; 278 | sqParams.min.pz.upperBound = this->min.pz.upperBound; 279 | sqParams.min.pz.type = this->min.pz.type; 280 | sqParams.min.pz.value = this->min.pz.value; 281 | 282 | sqParams.min.kx.lowerBound = this->min.kx.lowerBound; 283 | sqParams.min.kx.upperBound = this->min.kx.upperBound; 284 | sqParams.min.kx.type = this->min.kx.type; 285 | sqParams.min.kx.value = this->min.kx.value; 286 | 287 | sqParams.min.ky.lowerBound = this->min.ky.lowerBound; 288 | sqParams.min.ky.upperBound = this->min.ky.upperBound; 289 | sqParams.min.ky.type = this->min.ky.type; 290 | sqParams.min.ky.value = this->min.ky.value; 291 | sqParams.min.iterations = this->min.iterations; 292 | } 293 | 294 | 295 | /** 296 | * \brief Prints the superquadric parameters to an output stream 297 | */ 298 | friend std::ostream& operator<<(std::ostream &os, const SQParameters& sq) { 299 | os << ">>\t Superquadric Parameters:\n"; 300 | os << "\t [location] (px, py, pz) --> (" << sq.px << ", " << sq.py << ", " << sq.pz << ")\n"; 301 | os << "\t [dimensions] (a1, a2, a3) --> (" << sq.a1 << ", " << sq.a2 << ", " << sq.a3 << ")\n"; 302 | os << "\t [rotation] (phi, theta, psi) --> (" << sq.phi << ", " << sq.theta << ", " << sq.psi << ")\n"; 303 | os << "\t [shape] (e1, e2) --> (" << sq.e1 << ", " << sq.e2 << ")\n"; 304 | os << "\t [tapering] (kx, ky) --> (" << sq.kx << ", " << sq.ky << ")\n"; 305 | os << std::endl; 306 | 307 | return os; 308 | } 309 | 310 | }; 311 | 312 | 313 | } // ope 314 | 315 | #endif /* __SHAPEFITTINGTYPES_H__ */ -------------------------------------------------------------------------------- /TableObjectDetector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file TableObjectDetector.h 34 | * \author Nicolas Burrus, customized by Kester Duncan 35 | * \note Adapted from ntk's TableObjectDetector class 36 | */ 37 | #ifndef __TABLE_OBJECT_DETECTOR_H__ 38 | #define __TABLE_OBJECT_DETECTOR_H__ 39 | 40 | #include "pcl/filters/voxel_grid.h" 41 | #include "pcl/filters/passthrough.h" 42 | #include "pcl/filters/extract_indices.h" 43 | #include "pcl/features/normal_3d.h" 44 | #include "pcl/kdtree/kdtree.h" 45 | #include "pcl/kdtree/kdtree_flann.h" 46 | #include "pcl/sample_consensus/method_types.h" 47 | #include "pcl/sample_consensus/model_types.h" 48 | #include "pcl/segmentation/sac_segmentation.h" 49 | #include "pcl/filters/project_inliers.h" 50 | #include "pcl/surface/convex_hull.h" 51 | #include "pcl/segmentation/extract_polygonal_prism_data.h" 52 | #include "pcl/segmentation/extract_clusters.h" 53 | 54 | #include "Plane.h" 55 | #include "OPESettings.h" 56 | 57 | namespace ope { 58 | 59 | /** 60 | * \brief Detects clusters lying on a flat table 61 | * \note This class is adapted from ntk's TableObjectDetector 62 | * \warning Ensure that the table is large enough to be distinguished from the ground plane 63 | */ 64 | template 65 | class TableObjectDetector { 66 | public: 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 68 | 69 | typedef PointType Point; 70 | typedef typename pcl::PointCloud PointCloud; 71 | typedef typename PointCloud::ConstPtr PointCloudConstPtr; 72 | typedef typename pcl::search::KdTree::Ptr KdTreePtr; // Only if pcl version > 1.2.0 73 | typedef typename std::vector > PointVector; 74 | 75 | public: 76 | /// Constructor 77 | TableObjectDetector (); 78 | 79 | /// Initialize the properties of the TableObjectDetector 80 | void initialize(); 81 | 82 | public: 83 | /// Gets the voxel size used for downsampling 84 | float voxelSize() const { return downsample_leaf_objects_; } 85 | 86 | 87 | /// Sets the voxel size used for object downsampling. Default size is 0.3 cm. 88 | void setObjectVoxelSize(float s = 0.003f) { downsample_leaf_objects_ = s; } 89 | 90 | 91 | /// Sets the voxel size used for background downsampling. Default size is 1.0 cm. 92 | void setBackgroundVoxelSize(float s = 0.01) { downsample_leaf_ = s; } 93 | 94 | 95 | /// Sets the depth limits for processing 96 | void setDepthLimits(float min_z = 0.2f, float max_z = 1.8f) { min_z_bounds_ = min_z; max_z_bounds_ = max_z; } 97 | 98 | 99 | /// Sets the height limits for detected objects. 100 | void setObjectHeightLimits(float min_h = 0.01f, float max_h = 0.50f) { object_min_height_ = min_h; object_max_height_ = max_h; } 101 | 102 | 103 | /// Sets the threshold for an object's distance from the plane (table) 104 | void setMaxDistToPlane(float d) { m_max_dist_to_plane = d; } 105 | 106 | public: 107 | /* 108 | * \brief Detects the objects located on a flat table. 109 | * \return true if at least one object and plane are detected 110 | * \param - the point cloud that is used for detection 111 | */ 112 | bool detect(PointCloudConstPtr cloud); 113 | 114 | public: 115 | /// Gets a constant reference to the plane properties 116 | const Plane& plane() const { return m_plane; }; 117 | 118 | /// Gets a constant reference to the list of objects detected on the table 119 | const std::vector < PointVector >& objectClusters() const { return m_object_clusters; } 120 | 121 | /// Gets a constant pointer to the table points 122 | PointCloudConstPtr tableInliers() const { return table_projected_; } 123 | 124 | private: 125 | // PCL objects used for detection 126 | KdTreePtr normals_tree_; 127 | KdTreePtr clusters_tree_; 128 | pcl::PassThrough pass_; 129 | pcl::VoxelGrid grid_; 130 | pcl::VoxelGrid grid_objects_; 131 | pcl::NormalEstimation n3d_; 132 | pcl::SACSegmentationFromNormals seg_; 133 | pcl::ProjectInliers proj_; 134 | pcl::ConvexHull hull_; 135 | pcl::ExtractPolygonalPrismData prism_; 136 | pcl::EuclideanClusterExtraction cluster_; 137 | 138 | double downsample_leaf_; 139 | double downsample_leaf_objects_; 140 | int k_; 141 | double min_z_bounds_; 142 | double max_z_bounds_; 143 | double sac_distance_threshold_; 144 | double normal_distance_weight_; 145 | 146 | // Min / Max height from the table plane object points will be considered from/to 147 | double object_min_height_, object_max_height_; 148 | // Object cluster tolerance and minimum cluster size 149 | double object_cluster_tolerance_, object_cluster_min_size_; 150 | // Maximal distance between the object and the plane. 151 | double m_max_dist_to_plane; 152 | 153 | // The raw, input point cloud data 154 | PointCloudConstPtr cloud_; 155 | 156 | // The filtered and downsampled point cloud data 157 | PointCloudConstPtr cloud_filtered_, cloud_downsampled_; 158 | 159 | // The resultant estimated point cloud normals for \a cloud_filtered_ 160 | pcl::PointCloud::ConstPtr cloud_normals_; 161 | 162 | // The vector of indices from cloud_filtered_ that represent the planar table component 163 | pcl::PointIndices::ConstPtr table_inliers_; 164 | 165 | // The model coefficients of the planar table component 166 | pcl::ModelCoefficients::ConstPtr table_coefficients_; 167 | 168 | // The set of point inliers projected on the planar table component from \a cloud_filtered_ 169 | PointCloudConstPtr table_projected_; 170 | 171 | // The convex hull of \a table_projected_ 172 | PointCloudConstPtr table_hull_; 173 | 174 | // The remaining of the \a cloud_filtered_ which lies inside the \a table_hull_ polygon 175 | PointCloudConstPtr cloud_objects_, cloud_objects_downsampled_; 176 | 177 | /// Represents the table plane 178 | Plane m_plane; 179 | 180 | /// The object cluster which are found after detection 181 | std::vector< PointVector > m_object_clusters; 182 | 183 | /// Relevant settings 184 | OPESettings settings; 185 | 186 | }; 187 | 188 | 189 | 190 | } /* ope */ 191 | 192 | 193 | /* 194 | * Necessary for template implementations to be found with the declarations 195 | * by the compiler when creating a template instance. 196 | */ 197 | #include "TableObjectDetector.hpp" 198 | 199 | #endif /* __TABLE_OBJECT_DETECTOR_H__ */ 200 | 201 | -------------------------------------------------------------------------------- /TableObjectDetector.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * \file TableObjectDetector.hpp 3 | * \author Nicolas Burrus, altered by Kester Duncan 4 | * \note Adapted from ntk's TableObjectDetector 5 | * 6 | * \details This implementation file is used to separate the declared templates from 7 | * the implementation but at the same time, make the implementation of the templates 8 | * visible to the compiler. This is so because if the implementation is done in a 9 | * *.cpp file, the compiler cannot explicitly instantiate a new class given the 10 | * template argument. The implementation must be found with the declaration, which 11 | * is a stupid but necessary C++ limitation. 12 | */ 13 | #include 14 | #include "TableObjectDetector.h" 15 | 16 | 17 | namespace ope { 18 | 19 | template 20 | TableObjectDetector :: TableObjectDetector() : m_max_dist_to_plane(0.03), settings(OPESettings()) { 21 | 22 | // ---[ Create all PCL objects and set their parameters 23 | setObjectVoxelSize(settings.objVoxelSize); 24 | setBackgroundVoxelSize(); 25 | setDepthLimits(settings.minTgtDepth, settings.maxTgtDepth); 26 | setObjectHeightLimits(settings.minObjHeight, settings.maxObjHeight); 27 | 28 | // Normal estimation parameters 29 | k_ = 15; // 50 k-neighbors by default 30 | 31 | // Table model fitting parameters 32 | sac_distance_threshold_ = 0.01; // 1cm 33 | 34 | // Don't know ? 35 | normal_distance_weight_ = 0.1; 36 | 37 | // Clustering parameters 38 | object_cluster_tolerance_ = 0.05; // 5cm between two objects 39 | object_cluster_min_size_ = 100; // 100 points per object cluster 40 | } 41 | 42 | 43 | template 44 | void TableObjectDetector :: initialize() { 45 | grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_); 46 | grid_objects_.setLeafSize (downsample_leaf_objects_, downsample_leaf_objects_, downsample_leaf_objects_); 47 | grid_.setFilterFieldName ("z"); 48 | pass_.setFilterFieldName ("z"); 49 | 50 | grid_.setFilterLimits (min_z_bounds_, max_z_bounds_); 51 | pass_.setFilterLimits (min_z_bounds_, max_z_bounds_); 52 | grid_.setDownsampleAllData (false); 53 | grid_objects_.setDownsampleAllData (false); 54 | 55 | // Only works if PCL version > 1.2.0 56 | normals_tree_ = boost::make_shared > (); 57 | clusters_tree_ = boost::make_shared > (); 58 | clusters_tree_->setEpsilon (1); 59 | 60 | n3d_.setKSearch (k_); 61 | n3d_.setSearchMethod (normals_tree_); 62 | 63 | normal_distance_weight_ = 0.1; 64 | 65 | seg_.setNormalDistanceWeight (normal_distance_weight_); 66 | seg_.setOptimizeCoefficients (true); 67 | seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 68 | seg_.setMethodType (pcl::SAC_RANSAC); 69 | seg_.setProbability (0.99); 70 | seg_.setDistanceThreshold (sac_distance_threshold_); 71 | seg_.setMaxIterations (10000); 72 | 73 | proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE); 74 | 75 | prism_.setHeightLimits (object_min_height_, object_max_height_); 76 | 77 | cluster_.setClusterTolerance (object_cluster_tolerance_); 78 | cluster_.setMinClusterSize (object_cluster_min_size_); 79 | cluster_.setSearchMethod (clusters_tree_); 80 | } 81 | 82 | 83 | template 84 | bool TableObjectDetector :: detect(PointCloudConstPtr cloud) { 85 | m_object_clusters.clear(); 86 | initialize(); 87 | 88 | // ---[ Convert the dataset 89 | cloud_ = cloud; 90 | 91 | // ---[ Create the voxel grid 92 | pcl::PointCloud cloud_filtered; 93 | pass_.setInputCloud (cloud_); 94 | pass_.filter (cloud_filtered); 95 | cloud_filtered_.reset (new pcl::PointCloud (cloud_filtered)); 96 | 97 | pcl::PointCloud cloud_downsampled; 98 | grid_.setInputCloud (cloud_filtered_); 99 | grid_.filter (cloud_downsampled); 100 | cloud_downsampled_.reset (new pcl::PointCloud (cloud_downsampled)); 101 | 102 | if ((int) cloud_filtered_->points.size () < k_) { 103 | // This means that filtering returned very few points 104 | return false; 105 | } 106 | 107 | // ---[ Estimate the point normals 108 | pcl::PointCloud cloud_normals; 109 | n3d_.setInputCloud (cloud_downsampled_); 110 | n3d_.compute (cloud_normals); 111 | cloud_normals_.reset (new pcl::PointCloud (cloud_normals)); 112 | 113 | // ---[ Perform segmentation 114 | pcl::PointIndices table_inliers; pcl::ModelCoefficients table_coefficients; 115 | seg_.setInputCloud (cloud_downsampled_); 116 | seg_.setInputNormals (cloud_normals_); 117 | seg_.segment (table_inliers, table_coefficients); 118 | table_inliers_.reset (new pcl::PointIndices (table_inliers)); 119 | table_coefficients_.reset (new pcl::ModelCoefficients (table_coefficients)); 120 | 121 | if (table_inliers_->indices.size () == 0) 122 | return false; 123 | 124 | m_plane = Plane (table_coefficients.values[0], 125 | table_coefficients.values[1], 126 | table_coefficients.values[2], 127 | table_coefficients.values[3]); 128 | 129 | // ---[ Extract the table 130 | pcl::PointCloud table_projected; 131 | proj_.setInputCloud (cloud_downsampled_); 132 | proj_.setIndices (table_inliers_); 133 | proj_.setModelCoefficients (table_coefficients_); 134 | proj_.filter (table_projected); 135 | table_projected_.reset (new pcl::PointCloud (table_projected)); 136 | 137 | // ---[ Estimate the convex hull 138 | pcl::PointCloud table_hull; 139 | hull_.setDimension(2); 140 | hull_.setInputCloud (table_projected_); 141 | hull_.reconstruct (table_hull); 142 | table_hull_.reset (new pcl::PointCloud (table_hull)); 143 | 144 | // ---[ Get the objects on top of the table 145 | pcl::PointIndices cloud_object_indices; 146 | prism_.setInputCloud (cloud_filtered_); 147 | prism_.setInputPlanarHull (table_hull_); 148 | prism_.segment (cloud_object_indices); 149 | 150 | pcl::PointCloud cloud_objects; 151 | pcl::ExtractIndices extract_object_indices; 152 | extract_object_indices.setInputCloud (cloud_filtered_); 153 | extract_object_indices.setIndices (boost::make_shared (cloud_object_indices)); 154 | extract_object_indices.filter (cloud_objects); 155 | cloud_objects_.reset (new pcl::PointCloud (cloud_objects)); 156 | 157 | if (cloud_objects.points.size () == 0) 158 | return false; 159 | 160 | // ---[ Downsample the points 161 | #if 0 162 | pcl::PointCloud cloud_objects_downsampled; 163 | grid_objects_.setInputCloud (cloud_objects_); 164 | grid_objects_.filter (cloud_objects_downsampled); 165 | cloud_objects_downsampled_.reset (new pcl::PointCloud (cloud_objects_downsampled)); 166 | 167 | #endif 168 | 169 | // ---[ Split the objects into Euclidean clusters 170 | std::vector< pcl::PointIndices > object_clusters; 171 | cluster_.setInputCloud (cloud_objects_); 172 | cluster_.extract (object_clusters); 173 | 174 | 175 | for (size_t i = 0; i < object_clusters.size (); ++i) { 176 | std::vector > object_points; 177 | 178 | for (size_t k = 0; k < object_clusters[i].indices.size(); ++k) { 179 | int index = object_clusters[i].indices[k]; 180 | Point p = cloud_objects_->points[index]; 181 | object_points.push_back(p); 182 | } 183 | 184 | float min_dist_to_plane = FLT_MAX; 185 | for (int j = 0; j < object_points.size(); ++j) { 186 | pcl::PointXYZ pobj; 187 | pobj.x = object_points[j].x; 188 | pobj.y = object_points[j].y; 189 | pobj.z = object_points[j].z; 190 | 191 | min_dist_to_plane = std::min(plane().distanceToPlane(pobj), min_dist_to_plane); 192 | } 193 | 194 | if (min_dist_to_plane > m_max_dist_to_plane) 195 | continue; 196 | 197 | m_object_clusters.push_back(object_points); 198 | } 199 | 200 | return true; 201 | } 202 | 203 | } /* ope */ -------------------------------------------------------------------------------- /TableObjectModeler.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Object Pose Estimation (OPE) - www.cse.usf.edu/kkduncan/ope 5 | * Copyright (c) 2013, Kester Duncan 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | * \file TableObjectModeler.h 34 | * \brief Uses the output of the TableObjectDetector to create generic object models 35 | * \author Kester Duncan 36 | */ 37 | #pragma once 38 | #ifndef __TABLE_OBJECT_MODELER_H__ 39 | #define __TABLE_OBJECT_MODELER_H__ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include "TableObjectDetector.h" 46 | 47 | 48 | namespace ope { 49 | 50 | /** 51 | * \brief Defines a 3D bounding box around a point cloud 52 | */ 53 | class BoundingBox { 54 | public: 55 | BoundingBox() : minX(999), maxX(-999), minY(999), maxY(-999), minZ(999), maxZ(-999), width(0), height(0), depth(0) {} 56 | BoundingBox(float _minX, float _maxX, float _minY, float _maxY, float _minZ, float _maxZ, float _w, float _h, float _d) 57 | : minX(_minX), maxX(_maxX), minY(_minY), maxY(_maxY), minZ(_minZ), maxZ(_maxZ), width(_w), height(_h), depth(_d) {} 58 | 59 | 60 | inline bool isEmpty() const { 61 | return (width < 0 || height < 0 || depth < 0); 62 | } 63 | 64 | 65 | inline pcl::PointXYZ centroid() const { 66 | return pcl::PointXYZ((minX + maxX) / 2.0f, (minY + maxY) / 2.0f, (minZ + maxZ) / 2.0f); 67 | } 68 | 69 | 70 | inline bool isPointInside(const pcl::PointXYZ& p) const { 71 | return ((p.x >= minX) && (p.x <= maxX) && (p.y >= minY) && (p.y <= maxY) && (p.z >= minZ) && (p.z <= maxZ)); 72 | } 73 | 74 | inline bool isPointInside2D(const int& x, const int& y) { 75 | bool isInside = false; 76 | int pixelOffset = 5; 77 | 78 | if (x >= (xPos() - pixelOffset) && x <= (xPos() + pixelOffset) && 79 | y >= (yPos() - pixelOffset) && y <= (yPos() + pixelOffset)) { 80 | isInside = true; 81 | } 82 | return isInside; 83 | } 84 | 85 | 86 | inline float volume() const { 87 | return (width * height * depth); 88 | } 89 | 90 | 91 | inline void update(float x, float y, float z) { 92 | if (x < minX) minX = x; 93 | if (y < minY) minY = y; 94 | if (z < minZ) minZ = z; 95 | 96 | if (x > maxX) maxX = x; 97 | if (y > maxY) maxY = y; 98 | if (z > maxZ) maxZ = z; 99 | 100 | width = maxX - minX; 101 | height = maxY - minY; 102 | depth = maxZ - minZ; 103 | } 104 | 105 | 106 | inline int xPos() const { 107 | // 2D screen position 108 | float xPos = (525.0f * minX) / minZ; 109 | xPos += 320; 110 | 111 | return ((int) xPos); 112 | } 113 | 114 | 115 | inline int yPos() const { 116 | // 2D screen position 117 | float yPos = (525.0f * minY) / minZ; 118 | yPos += 240; 119 | 120 | return ((int) yPos); 121 | } 122 | 123 | 124 | friend std::ostream& operator<<(std::ostream &os, const BoundingBox& b) { 125 | os << "\n\tBounding Box Properties:\n"; 126 | os << "\t\t (min X, max X) --> (" << b.minX << ", " << b.maxX << ")\n"; 127 | os << "\t\t (min Y, max Y) --> (" << b.minY << ", " << b.maxY << ")\n"; 128 | os << "\t\t (min Z, max Z) --> (" << b.minZ << ", " << b.maxZ << ")\n"; 129 | os << "\t\t (width, height, depth) --> (" << b.width << ", " << b.height << ", " << b.depth << ")\n"; 130 | os << "\t\t (x position, y position) --> (" << b.xPos() << ", " << b.yPos() << ")\n"; 131 | os << std::endl; 132 | 133 | return os; 134 | } 135 | 136 | 137 | public: 138 | float minX, maxX; 139 | float minY, maxY; 140 | float minZ, maxZ; 141 | float width; 142 | float height; 143 | float depth; 144 | 145 | }; // BoundingBox 146 | 147 | 148 | 149 | 150 | /** 151 | * \brief Models object hypotheses found on a table 152 | */ 153 | template 154 | class TableObjectModel { 155 | public: 156 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 157 | 158 | pcl::PointCloud objectCloud; 159 | BoundingBox box; 160 | size_t objectId; ///< A cluster object's unique id according to the order it was detected 161 | std::string name; ///< Cluster's identifier 162 | 163 | public: 164 | TableObjectModel() : objectId(999), name("Unknown") {} 165 | ~TableObjectModel() { 166 | if (!objectCloud.empty()) { 167 | objectCloud.clear(); 168 | } 169 | } 170 | 171 | 172 | /** 173 | * \brief Create the object model 174 | */ 175 | void create(const TableObjectDetector< PointType >& detector, const size_t& clusterId) { 176 | if (clusterId >= detector.objectClusters().size()) { 177 | std::cerr << "TableObjectModeler: Invalid cluster!" << std::endl; 178 | } 179 | 180 | pcl::PointCloud< PointType >::Ptr tempCloudPtr (new pcl::PointCloud< PointType>); 181 | 182 | objectId = clusterId; 183 | name = "Object_"; 184 | name += boost::lexical_cast(objectId); 185 | 186 | size_t numPoints = detector.objectClusters()[clusterId].size(); 187 | tempCloudPtr->reserve(numPoints); 188 | 189 | for (size_t i = 0; i < numPoints; ++i) { 190 | PointType p = detector.objectClusters()[clusterId][i]; 191 | tempCloudPtr->push_back(p); 192 | } 193 | 194 | pcl::StatisticalOutlierRemoval< PointType > sor (true); 195 | sor.setInputCloud (tempCloudPtr); 196 | sor.setMeanK(8); 197 | sor.setStddevMulThresh(1.0); 198 | sor.filter(objectCloud); 199 | 200 | for (size_t i = 0; i < objectCloud.size(); ++i) { 201 | PointType p = objectCloud.points[i]; 202 | box.update(p.x, p.y, p.z); 203 | } 204 | } 205 | 206 | 207 | 208 | }; // TableObjectModel 209 | 210 | 211 | 212 | /** 213 | * \brief Generate point cloud models for objects detected on a table 214 | */ 215 | template 216 | class ObjectModelGenerator { 217 | public: 218 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 219 | 220 | typedef typename pcl::PointCloud PointCloud; 221 | typedef typename PointCloud::Ptr PointCloudPtr; 222 | typedef typename PointCloud::ConstPtr PointCloudConstPtr; 223 | 224 | private: 225 | PointCloudConstPtr srcCloudPtr; 226 | 227 | public: 228 | /** 229 | * \brief List of object models 230 | * 231 | * \note This declaration is rather messy but it is required if we desire to use ANYTHING Eigen. All 232 | * Eigen objects are 16-byte aligned. Therefore we must use Eigen's memory allocator when using standard 233 | * containers. 234 | */ 235 | std::vector < TableObjectModel< PointType >, Eigen::aligned_allocator< TableObjectModel< PointType > > > objects; 236 | 237 | 238 | ObjectModelGenerator() : detectedObjects(false) { } 239 | ObjectModelGenerator(PointCloudConstPtr ptCloudPtr) { 240 | setSourceCloud(ptCloudPtr); 241 | } 242 | 243 | ~ObjectModelGenerator() { 244 | if(!objects.empty()) { 245 | objects.clear(); 246 | } 247 | } 248 | 249 | 250 | void setSourceCloud(PointCloudConstPtr ptCloudPtr) { 251 | srcCloudPtr = ptCloudPtr; 252 | 253 | } 254 | 255 | 256 | bool generateObjectModels(const OPESettings& settings = OPESettings()) { 257 | TableObjectDetector< PointType > detector; 258 | bool detectedObjects = detector.detect(srcCloudPtr); 259 | 260 | if (settings.verbose) { 261 | std::cout << ">> Generating object models" << std::endl; 262 | 263 | if (detectedObjects) { 264 | std::cout << ">>\t " << detector.objectClusters().size() << " object(s) detected" << endl; 265 | } else { 266 | std::cout << ">>\t " << "No objects were detected" << std::endl; 267 | } 268 | } 269 | 270 | for (size_t i = 0; i < detector.objectClusters().size(); ++i) { 271 | TableObjectModel< PointType > objectModel; 272 | objectModel.create(detector, i); 273 | objects.push_back(objectModel); 274 | } 275 | 276 | if (detector.objectClusters().size() > 0) { 277 | detectedObjects = true; 278 | } else { 279 | detectedObjects = false; 280 | } 281 | 282 | return detectedObjects; 283 | } 284 | 285 | 286 | std::vector getBoundingBoxes() { 287 | std::vector objectBoxes; 288 | 289 | for (size_t i = 0; i < objects.size(); ++i) { 290 | objectBoxes.push_back(objects[i].box); 291 | } 292 | 293 | return objectBoxes; 294 | } 295 | 296 | }; // ObjectModelGenerator 297 | 298 | 299 | 300 | } /* ope */ 301 | 302 | 303 | #endif /* __TABLE_OBJECT_MODELER_H__ */ --------------------------------------------------------------------------------