Subversion Repositories seema-scanner

Rev

Rev 196 | Rev 198 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 196 Rev 197
Line 11... Line 11...
11 
struct CircleResidual {
 11 
struct CircleResidual {
12 
  CircleResidual(std::vector<cv::Point3f> _pointsOnArc)
 12 
  CircleResidual(std::vector<cv::Point3f> _pointsOnArc)
13 
      : pointsOnArc(_pointsOnArc) {}
 13 
      : pointsOnArc(_pointsOnArc) {}
14 
 
 14 
 
15 
  template <typename T>
 15 
  template <typename T>
16 
  bool operator()(const T* const point_x, const T* const point_y, const T* const point_z,
 16 
  bool operator()(const T* point, const T* axis, T* residual) const {
17 
                  const T* const axis_x, const T* const axis_y, const T* const axis_z,
 - 
 
18 
                  T* residual) const {
 - 
 
19 
 
 17 
 
20 
    cv::Vec3f axis(*axis_x, *axis_y, *axis_z);
 18 
    T axisSqNorm = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];
21 
    cv::Vec3f point(*point_x, *point_y, *point_z);
 - 
 
22 
 
 19 
 
23 
    unsigned int l = pointsOnArc.size();
 20 
    unsigned int l = pointsOnArc.size();
24 
    std::vector<float> dI(l);
 21 
    std::vector<T> dI(l);
- 
 
 22 
 
- 
 
 23 
    // note, this is automatically initialized to 0
- 
 
 24 
    T sum(0.0);
- 
 
 25 
 
25 
    for(unsigned int i=0; i<l; i++){
 26 
    for(unsigned int i=0; i<l; i++){
26 
      cv::Vec3f p = cv::Vec3f(pointsOnArc[i]);
 27 
      cv::Point3d p = pointsOnArc[i];
- 
 
 28 
      //T p[3] = {pointsOnArc[i].x, pointsOnArc[i].y, pointsOnArc[i].z};
- 
 
 29 
 
27 
      // point to line distance
 30 
      // point to line distance
28 
      dI[i] = cv::norm((point-p)-(point-p).dot(axis)*axis);
 31 
      T dotProd = (point[0]-p.x)*axis[0] + (point[1]-p.y)*axis[1] + (point[2]-p.z)*axis[2];
29 
    }
 - 
 
30 
    float sum = std::accumulate(dI.begin(), dI.end(), 0.0);
 32 
      T dIx = point[0] - p.x - (dotProd*axis[0]/axisSqNorm);
31 
    float mean = sum / dI.size();
 33 
      T dIy = point[1] - p.y - (dotProd*axis[1]/axisSqNorm);
32 
    float meanDev = 0;
 34 
      T dIz = point[2] - p.z - (dotProd*axis[2]/axisSqNorm);
33 
    for(unsigned int k=0; k<l; k++){
 35 
      dI[i] = ceres::sqrt(dIx*dIx + dIy*dIy + dIz*dIz);
- 
 
 36 
 
34 
      meanDev += std::abs(dI[k] - mean);
 37 
      sum += dI[i];
35 
    }
 38 
    }
36 
    meanDev /= l;
 - 
 
37 
 
 39 
 
- 
 
 40 
    T mean = sum / double(l);
- 
 
 41 
 
- 
 
 42 
    for(unsigned int i=0; i<l; i++){
38 
    residual[0] = T(meanDev);
 43 
        residual[i] = dI[i] - mean;
- 
 
 44 
    }
39 
 
 45 
 
40 
    return true;
 46 
    return true;
41 
  }
 47 
  }
42 
 
 48 
 
43 
 private:
 49 
 private:
Line 181... Line 187...
181 
    point[0] = a;
 187 
    point[0] = a;
182 
    point[1] = b;
 188 
    point[1] = b;
183 
    point[2] = c;
 189 
    point[2] = c;
184 
 
 190 
 
185 
    // Non-linear optimization using Ceres
 191 
    // Non-linear optimization using Ceres
186 
    double point_x = point[0];
 - 
 
187 
    double point_y = point[1];
 192 
    double pointArray[] = {point[0], point[1], point[2]};
188 
    double point_z = point[2];
 - 
 
189 
    double axis_x = axis[0];
 - 
 
190 
    double axis_y = axis[1];
 193 
    double axisArray[] = {axis[0], axis[1], axis[2]};
191 
    double axis_z = axis[2];
 - 
 
192 
 
 194 
 
193 
    ceres::Problem problem;
 195 
    ceres::Problem problem;
194 
    // loop through saddle points
 196 
    // loop through saddle points
195 
    for(unsigned int idx=0; idx<mn; idx++){
 197 
    for(unsigned int idx=0; idx<mn; idx++){
196 
        std::vector<cv::Point3f> pointsOnArch(l);
 198 
        std::vector<cv::Point3f> pointsOnArch(l);
197 
        for(int k=0; k<l; k++){
 199 
        for(int k=0; k<l; k++){
198 
            pointsOnArch[k] = Qcam[k][idx];
 200 
            pointsOnArch[k] = Qcam[k][idx];
199 
        }
 201 
        }
200 
        ceres::CostFunction* cost_function =
 202 
        ceres::CostFunction* cost_function =
201 
           new ceres::NumericDiffCostFunction<CircleResidual, ceres::CENTRAL, 1, 1, 1, 1, 1, 1, 1>(
 203 
           new ceres::AutoDiffCostFunction<CircleResidual, ceres::DYNAMIC, 3, 3>(
202 
               new CircleResidual(pointsOnArch));
 204 
               new CircleResidual(pointsOnArch), l);
203 
        problem.AddResidualBlock(cost_function, NULL, &point_x, &point_y, &point_z, &axis_x, &axis_y, &axis_z);
 205 
        problem.AddResidualBlock(cost_function, NULL, pointArray, axisArray);
204 
    }
 206 
    }
205 
 
 207 
 
206 
    // Run the solver!
 208 
    // Run the solver!
207 
    ceres::Solver::Options options;
 209 
    ceres::Solver::Options options;
208 
    options.linear_solver_type = ceres::DENSE_QR;
 210 
    options.linear_solver_type = ceres::DENSE_QR;
Line 210... Line 212...
210 
    ceres::Solver::Summary summary;
 212 
    ceres::Solver::Summary summary;
211 
    ceres::Solve(options, &problem, &summary);
 213 
    ceres::Solve(options, &problem, &summary);
212 
 
 214 
 
213 
    std::cout << summary.BriefReport() << "\n";
 215 
    std::cout << summary.BriefReport() << "\n";
214 
 
 216 
 
- 
 
 217 
    point = cv::Vec3f(pointArray[0], pointArray[1], pointArray[2]);
- 
 
 218 
    axis = cv::Vec3f(axisArray[0], axisArray[1], axisArray[2]);
- 
 
 219 
    axis /= cv::norm(axis);
215 
 
 220 
 
216 
    point = cv::Point3f(point_x, point_y, point_z);
 - 
 
217 
    axis = cv::Point3f(axis_x, axis_y, axis_z);
 - 
 
218 
 
 221 
 
219 
 
 - 
 
220 
    // Error estimate (mean 3D point deviations from circles around rotation axis)
 222 
    // Error estimate (sum of squared differences)
221 
    error = 0;
 223 
    error = 0;
222 
    // loop through saddle points
 224 
    // loop through saddle points
223 
    for(unsigned int idx=0; idx<mn; idx++){
 225 
    for(unsigned int idx=0; idx<mn; idx++){
224 
 
 226 
 
225 
        // vector of distances from rotation axis
 227 
        // vector of distances from rotation axis
Line 230... Line 232...
230 
            // point to line distance
 232 
            // point to line distance
231 
            dI[k] = cv::norm((point-p)-(point-p).dot(axis)*axis);
 233 
            dI[k] = cv::norm((point-p)-(point-p).dot(axis)*axis);
232 
        }
 234 
        }
233 
        float sum = std::accumulate(dI.begin(), dI.end(), 0.0);
 235 
        float sum = std::accumulate(dI.begin(), dI.end(), 0.0);
234 
        float mean = sum / dI.size();
 236 
        float mean = sum / dI.size();
235 
        float meanDev = 0;
 237 
        float sumSqDev = 0;
236 
        for(int k=0; k<l; k++){
 238 
        for(int k=0; k<l; k++){
237 
            meanDev += std::abs(dI[k] - mean);
 239 
            sumSqDev += (dI[k] - mean)*(dI[k] - mean);
238 
        }
 240 
        }
239 
        meanDev /= l;
 241 
        //meanDev /= l;
240 
        //std::cout << meanDev << std::endl;
 242 
        //std::cout << meanDev << std::endl;
241 
        error += meanDev;
 243 
        error += sumSqDev;
242 
    }
 244 
    }
243 
    error /= mn;
 245 
    //error /= mn;
244 
 
 - 
 
245 
}
 246 
}
246 
 
 247 
 
247 
void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
 248 
void SMCalibrationWorker::performCalibration(std::vector<SMCalibrationSet> calibrationData){
248 
 
 249 
 
249 
    QSettings settings;
 250 
    QSettings settings;