global_optimization_pdm.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012-, Open Perception, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder(s) nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * 
00035  *
00036  */
00037 
00038 #include <pcl/surface/on_nurbs/global_optimization_pdm.h>
00039 #include <pcl/surface/on_nurbs/closing_boundary.h>
00040 #include <stdexcept>
00041 
00042 #undef DEBUG
00043 
00044 using namespace pcl;
00045 using namespace on_nurbs;
00046 using namespace Eigen;
00047 
00048 GlobalOptimization::GlobalOptimization (const std::vector<NurbsDataSurface*> &data,
00049                                         const std::vector<ON_NurbsSurface*> &nurbs)
00050 {
00051   if (data.empty () || data.size () != nurbs.size ())
00052     printf ("[GlobalOptimization::GlobalOptimization] Error, data empty or size does not match.\n");
00053 
00054   m_data = data;
00055   m_nurbs = nurbs;
00056 
00057   im_max_steps = 100;
00058   im_accuracy = 1e-5;
00059 
00060   m_quiet = true;
00061 }
00062 
00063 void
00064 GlobalOptimization::setCommonBoundary (const vector_vec3d &boundary, const vector_vec2i &nurbs_indices)
00065 {
00066   if (boundary.empty () || boundary.size () != nurbs_indices.size ())
00067   {
00068     printf ("[GlobalOptimization::setCommonBoundary] Error, common boundary empty or size does not match.\n");
00069     return;
00070   }
00071 }
00072 
00073 void
00074 GlobalOptimization::assemble (Parameter params)
00075 {
00076   // determine number of rows of matrix
00077   m_ncols = 0;
00078   unsigned nnurbs = static_cast<unsigned> (m_nurbs.size ());
00079   unsigned nInt (0), nBnd (0), nCageRegInt (0), nCageRegBnd (0), nCommonBnd (0), nCommonPar (0);
00080   for (unsigned i = 0; i < nnurbs; i++)
00081   {
00082     nInt += static_cast<unsigned> (m_data[i]->interior.size ());
00083     nBnd += static_cast<unsigned> (m_data[i]->boundary.size ());
00084     nCommonBnd += static_cast<unsigned> (m_data[i]->common_boundary_point.size ());
00085     nCommonPar += static_cast<unsigned> (m_data[i]->common_idx.size ());
00086     nCageRegInt += (m_nurbs[i]->CVCount (0) - 2) * (m_nurbs[i]->CVCount (1) - 2);
00087     nCageRegBnd += 2 * (m_nurbs[i]->CVCount (0) - 1) + 2 * (m_nurbs[i]->CVCount (1) - 1);
00088     m_ncols += m_nurbs[i]->CVCount ();
00089   }
00090 
00091   m_nrows = 0;
00092   if (params.interior_weight > 0.0)
00093     m_nrows += nInt;
00094   if (params.boundary_weight > 0.0)
00095     m_nrows += nBnd;
00096   if (params.interior_smoothness > 0.0)
00097     m_nrows += nCageRegInt;
00098   if (params.boundary_smoothness > 0.0)
00099     m_nrows += nCageRegBnd;
00100   if (params.closing_weight > 0.0)
00101   {
00102     if (params.closing_samples > 0)
00103       m_nrows += (4 * params.closing_samples);
00104     else
00105       m_nrows += nBnd;
00106   }
00107   if (params.common_weight > 0.0)
00108   {
00109     m_nrows += nCommonBnd;
00110     m_nrows += nCommonPar;
00111   }
00112 
00113   if (!m_quiet)
00114     printf ("[GlobalOptimization::assemble] Size of matrix: %dx%d\n", m_nrows, m_ncols);
00115 
00116   m_solver.assign (m_nrows, m_ncols, 3);
00117 
00118   for (unsigned i = 0; i < m_data.size (); i++)
00119     m_data[i]->common_boundary_param.clear ();
00120 
00121   // assemble matrix
00122   unsigned row (0);
00123   int ncps (0);
00124 
00125   for (unsigned id = 0; id < nnurbs; id++)
00126   {
00127 
00128     // interior points should lie on surface
00129     assembleInteriorPoints (id, ncps, params.interior_weight, row);
00130 
00131     // boundary points should lie on boundary of surface
00132     assembleBoundaryPoints (id, ncps, params.boundary_weight, row);
00133 
00134     // cage regularisation
00135     assembleRegularisation (id, ncps, params.interior_smoothness, params.boundary_smoothness, row);
00136 
00137     // closing boundaries
00138     assembleClosingBoundaries (id, params.closing_samples, params.closing_sigma, params.closing_weight, row);
00139 
00140     // common boundaries
00141     assembleCommonBoundaries (id, params.common_weight, row);
00142 
00143     // common parameter points
00144     assembleCommonParams (id, params.common_weight, row);
00145 
00146     ncps += m_nurbs[id]->CVCount ();
00147   }
00148 }
00149 
00150 void
00151 GlobalOptimization::solve (double damp)
00152 {
00153   if (m_solver.solve ())
00154     updateSurf (damp);
00155 }
00156 
00157 void
00158 GlobalOptimization::updateSurf (double damp)
00159 {
00160   int ncps (0);
00161 
00162   for (unsigned i = 0; i < m_nurbs.size (); i++)
00163   {
00164     ON_NurbsSurface* nurbs = m_nurbs[i];
00165 
00166     int ncp = nurbs->CVCount ();
00167 
00168     for (int A = 0; A < ncp; A++)
00169     {
00170 
00171       int I = gl2gr (*nurbs, A);
00172       int J = gl2gc (*nurbs, A);
00173 
00174       ON_3dPoint cp_prev;
00175       nurbs->GetCV (I, J, cp_prev);
00176 
00177       ON_3dPoint cp;
00178       cp.x = cp_prev.x + damp * (m_solver.x (ncps + A, 0) - cp_prev.x);
00179       cp.y = cp_prev.y + damp * (m_solver.x (ncps + A, 1) - cp_prev.y);
00180       cp.z = cp_prev.z + damp * (m_solver.x (ncps + A, 2) - cp_prev.z);
00181 
00182       nurbs->SetCV (I, J, cp);
00183     }
00184 
00185     ncps += ncp;
00186   }
00187 }
00188 
00189 void
00190 GlobalOptimization::setInvMapParams (double im_max_steps, double im_accuracy)
00191 {
00192   this->im_max_steps = static_cast<int> (im_max_steps);
00193   this->im_accuracy = im_accuracy;
00194 }
00195 
00196 void
00197 GlobalOptimization::refine (unsigned id, int dim)
00198 {
00199   std::vector<double> xi;
00200   std::vector<double> elements = FittingSurface::getElementVector (*m_nurbs[id], dim);
00201 
00202   for (unsigned i = 0; i < elements.size () - 1; i++)
00203     xi.push_back (elements[i] + 0.5 * (elements[i + 1] - elements[i]));
00204 
00205   for (unsigned i = 0; i < xi.size (); i++)
00206   {
00207     m_nurbs[id]->InsertKnot (dim, xi[i], 1);
00208   }
00209 }
00210 
00211 void
00212 GlobalOptimization::assembleCommonParams (unsigned id1, double weight, unsigned &row)
00213 {
00214   if (weight <= 0.0)
00215     return;
00216 
00217   NurbsDataSurface *data = m_data[id1];
00218 
00219   for (size_t i = 0; i < data->common_idx.size (); i++)
00220     addParamConstraint (Eigen::Vector2i (id1, data->common_idx[i]), data->common_param1[i], data->common_param2[i],
00221                         weight, row);
00222 }
00223 
00224 void
00225 GlobalOptimization::assembleCommonBoundaries (unsigned id1, double weight, unsigned &row)
00226 {
00227   if (weight <= 0.0)
00228     return;
00229 
00230   //  double ds = 1.0 / (2.0 * sigma * sigma);
00231   NurbsDataSurface *data1 = m_data[id1];
00232   ON_NurbsSurface* nurbs1 = m_nurbs[id1];
00233 
00234   if (nurbs1->Order (0) != nurbs1->Order (1))
00235     printf ("[GlobalOptimization::assembleCommonBoundaries] Warning, order in u and v direction differ (nurbs1).\n");
00236 
00237   for (unsigned i = 0; i < data1->common_boundary_point.size (); i++)
00238   {
00239     Eigen::Vector3d p1, tu1, tv1, p2, tu2, tv2, t1, t2;
00240     Eigen::Vector2d params1, params2;
00241     double error1, error2;
00242     Eigen::Vector3d p0 = data1->common_boundary_point[i];
00243     Eigen::Vector2i id (id1, data1->common_boundary_idx[i]);
00244 
00245     if (id (1) < 0 || id (1) >= m_nurbs.size ())
00246       throw std::runtime_error (
00247                                 "[GlobalOptimization::assembleCommonBoundaries] Error, common boundary index out of bounds.\n");
00248 
00249     ON_NurbsSurface* nurbs2 = m_nurbs[id (1)];
00250     double w (1.0);
00251 
00252     if (nurbs2->Order (0) != nurbs2->Order (1))
00253       printf ("[GlobalOptimization::assembleCommonBoundaries] Warning, order in u and v direction differ (nurbs2).\n");
00254 
00255     if (nurbs1->Order (0) == nurbs2->Order (0))
00256     {
00257       params1 = FittingSurface::inverseMappingBoundary (*m_nurbs[id (0)], p0, error1, p1, tu1, tv1, im_max_steps,
00258                                                         im_accuracy, true);
00259       params2 = FittingSurface::inverseMappingBoundary (*m_nurbs[id (1)], p0, error2, p2, tu2, tv2, im_max_steps,
00260                                                         im_accuracy, true);
00261 
00262       if (params1 (0) == 0.0 || params1 (0) == 1.0)
00263         t1 = tv1;
00264       if (params1 (1) == 0.0 || params1 (1) == 1.0)
00265         t1 = tu1;
00266       if (params2 (0) == 0.0 || params2 (0) == 1.0)
00267         t2 = tv2;
00268       if (params2 (1) == 0.0 || params2 (1) == 1.0)
00269         t2 = tu2;
00270 
00271       t1.normalize ();
00272       t2.normalize ();
00273 
00274       // weight according to parallel-ness of boundaries
00275       w = t1.dot (t2);
00276       if (w < 0.0)
00277         w = -w;
00278     }
00279     else
00280     {
00281 
00282       if (nurbs1->Order (0) < nurbs2->Order (0))
00283       {
00284         params1 = FittingSurface::findClosestElementMidPoint (*m_nurbs[id (0)], p0);
00285         params1 = FittingSurface::inverseMapping (*m_nurbs[id (0)], p0, params1, error1, p1, tu1, tv1, im_max_steps,
00286                                                   im_accuracy, true);
00287         params2 = FittingSurface::inverseMappingBoundary (*m_nurbs[id (1)], p0, error2, p2, tu2, tv2, im_max_steps,
00288                                                           im_accuracy, true);
00289       }
00290       else
00291       {
00292         params1 = FittingSurface::inverseMappingBoundary (*m_nurbs[id (0)], p0, error1, p1, tu1, tv1, im_max_steps,
00293                                                           im_accuracy, true);
00294         params2 = FittingSurface::findClosestElementMidPoint (*m_nurbs[id (1)], p0);
00295         params2 = FittingSurface::inverseMapping (*m_nurbs[id (1)], p0, params2, error2, p2, tu2, tv2, im_max_steps,
00296                                                   im_accuracy);
00297       }
00298 
00299     }
00300 
00301     m_data[id (0)]->common_boundary_param.push_back (params1);
00302     m_data[id (1)]->common_boundary_param.push_back (params2);
00303 
00304     //    double error = (p1-p2).norm();
00305     //    double w = weight * exp(-(error * error) * ds);
00306     addParamConstraint (id, params1, params2, weight * w, row);
00307 
00308   }
00309 }
00310 
00311 void
00312 GlobalOptimization::assembleClosingBoundaries (unsigned id, unsigned samples, double sigma, double weight,
00313                                                unsigned &row)
00314 {
00315   if (weight <= 0.0 || samples <= 0 || sigma < 0.0)
00316     return;
00317 
00318   double ds = 1.0 / (2.0 * sigma * sigma);
00319 
00320   ON_NurbsSurface *nurbs1 = m_nurbs[id];
00321 
00322   // sample point list from nurbs1
00323   vector_vec3d boundary1, boundary2;
00324   vector_vec2d params1, params2;
00325   ClosingBoundary::sampleFromBoundary (nurbs1, boundary1, params1, samples);
00326 
00327   // for each other nurbs
00328   for (unsigned n2 = (id + 1); n2 < m_nurbs.size (); n2++)
00329   {
00330     ON_NurbsSurface *nurbs2 = m_nurbs[n2];
00331 
00332     // find closest point to boundary
00333     for (unsigned i = 0; i < boundary1.size (); i++)
00334     {
00335       double error;
00336       Eigen::Vector3d p, tu, tv;
00337       Eigen::Vector2d params;
00338       Eigen::Vector3d p0 = boundary1[i];
00339 
00340       params = FittingSurface::inverseMappingBoundary (*nurbs2, p0, error, p, tu, tv, im_max_steps, im_accuracy, true);
00341 
00342       boundary2.push_back (p);
00343       params2.push_back (params);
00344 
00345       //      double dist = (p - p0).norm();
00346       //      if (error < max_error && dist < max_dist) {
00347       double w = weight * exp (-(error * error) * ds);
00348       addParamConstraint (Eigen::Vector2i (id, n2), params1[i], params, w, row);
00349       //      }
00350     }
00351   }
00352 }
00353 
00354 void
00355 GlobalOptimization::assembleInteriorPoints (unsigned id, int ncps, double weight, unsigned &row)
00356 {
00357   if (weight <= 0.0)
00358     return;
00359 
00360   ON_NurbsSurface *nurbs = m_nurbs[id];
00361   NurbsDataSurface *data = m_data[id];
00362   unsigned nInt = static_cast<unsigned> (m_data[id]->interior.size ());
00363 
00364   // interior points should lie on surface
00365   data->interior_line_start.clear ();
00366   data->interior_line_end.clear ();
00367   data->interior_error.clear ();
00368   data->interior_normals.clear ();
00369   //  data->interior_param.clear();
00370 
00371   for (unsigned p = 0; p < nInt; p++)
00372   {
00373     Vector3d pcp;
00374     pcp (0) = data->interior[p] (0);
00375     pcp (1) = data->interior[p] (1);
00376     pcp (2) = data->interior[p] (2);
00377 
00378     // inverse mapping
00379     Vector2d params;
00380     Vector3d pt, tu, tv, n;
00381     double error;
00382     if (p < data->interior_param.size ())
00383     {
00384       params = FittingSurface::inverseMapping (*nurbs, pcp, data->interior_param[p], error, pt, tu, tv, im_max_steps,
00385                                                im_accuracy);
00386       data->interior_param[p] = params;
00387     }
00388     else
00389     {
00390       params = FittingSurface::findClosestElementMidPoint (*nurbs, pcp);
00391       params = FittingSurface::inverseMapping (*nurbs, pcp, params, error, pt, tu, tv, im_max_steps, im_accuracy);
00392       data->interior_param.push_back (params);
00393     }
00394     data->interior_error.push_back (error);
00395 
00396     n = tu.cross (tv);
00397     n.normalize ();
00398 
00399     data->interior_normals.push_back (n);
00400     data->interior_line_start.push_back (pcp);
00401     data->interior_line_end.push_back (pt);
00402 
00403     addPointConstraint (id, ncps, params, pcp, weight, row);
00404   }
00405 }
00406 
00407 void
00408 GlobalOptimization::assembleBoundaryPoints (unsigned id, int ncps, double weight, unsigned &row)
00409 {
00410   if (weight <= 0.0)
00411     return;
00412 
00413   ON_NurbsSurface *nurbs = m_nurbs[id];
00414   NurbsDataSurface *data = m_data[id];
00415   unsigned nBnd = static_cast<unsigned> (m_data[id]->boundary.size ());
00416 
00417   // interior points should lie on surface
00418   data->boundary_line_start.clear ();
00419   data->boundary_line_end.clear ();
00420   data->boundary_error.clear ();
00421   data->boundary_normals.clear ();
00422   data->boundary_param.clear ();
00423 
00424   for (unsigned p = 0; p < nBnd; p++)
00425   {
00426     Vector3d pcp;
00427     pcp (0) = data->boundary[p] (0);
00428     pcp (1) = data->boundary[p] (1);
00429     pcp (2) = data->boundary[p] (2);
00430 
00431     // inverse mapping
00432     Vector3d pt, tu, tv, n;
00433     double error;
00434 
00435     Vector2d params =
00436         FittingSurface::inverseMappingBoundary (*nurbs, pcp, error, pt, tu, tv, im_max_steps, im_accuracy);
00437     data->boundary_error.push_back (error);
00438 
00439     if (p < data->boundary_param.size ())
00440     {
00441       data->boundary_param[p] = params;
00442     }
00443     else
00444     {
00445       data->boundary_param.push_back (params);
00446     }
00447 
00448     n = tu.cross (tv);
00449     n.normalize ();
00450 
00451     data->boundary_normals.push_back (n);
00452     data->boundary_line_start.push_back (pcp);
00453     data->boundary_line_end.push_back (pt);
00454 
00455     addPointConstraint (id, ncps, params, pcp, weight, row);
00456   }
00457 }
00458 
00459 void
00460 GlobalOptimization::assembleRegularisation (unsigned id, int ncps, double wCageRegInt, double wCageRegBnd,
00461                                             unsigned &row)
00462 {
00463   if (wCageRegBnd <= 0.0 || wCageRegInt <= 0.0)
00464   {
00465     printf ("[GlobalOptimization::assembleRegularisation] Warning, no regularisation may lead "
00466       "to under-determined equation system. Add cage regularisation (smoothing) to avoid.\n");
00467   }
00468 
00469   if (wCageRegInt > 0.0)
00470     addCageInteriorRegularisation (id, ncps, wCageRegInt, row);
00471 
00472   if (wCageRegBnd > 0.0)
00473   {
00474     addCageBoundaryRegularisation (id, ncps, wCageRegBnd, NORTH, row);
00475     addCageBoundaryRegularisation (id, ncps, wCageRegBnd, SOUTH, row);
00476     addCageBoundaryRegularisation (id, ncps, wCageRegBnd, WEST, row);
00477     addCageBoundaryRegularisation (id, ncps, wCageRegBnd, EAST, row);
00478     addCageCornerRegularisation (id, ncps, wCageRegBnd * 2.0, row);
00479   }
00480 }
00481 
00482 void
00483 GlobalOptimization::addParamConstraint (const Eigen::Vector2i &id, const Eigen::Vector2d &params1,
00484                                         const Eigen::Vector2d &params2, double weight, unsigned &row)
00485 {
00486   vector_vec2d params;
00487   params.push_back (params1);
00488   params.push_back (params2);
00489 
00490   for (unsigned n = 0; n < 2; n++)
00491   {
00492     ON_NurbsSurface* nurbs = m_nurbs[id (n)];
00493 
00494     double *N0 = new double[nurbs->Order (0) * nurbs->Order (0)];
00495     double *N1 = new double[nurbs->Order (1) * nurbs->Order (1)];
00496 
00497     int E = ON_NurbsSpanIndex (nurbs->Order (0), nurbs->CVCount (0), nurbs->m_knot[0], params[n] (0), 0, 0);
00498     int F = ON_NurbsSpanIndex (nurbs->Order (1), nurbs->CVCount (1), nurbs->m_knot[1], params[n] (1), 0, 0);
00499     //    int E = ntools.E(params[n](0));
00500     //    int F = ntools.F(params[n](1));
00501 
00502     m_solver.f (row, 0, 0.0);
00503     m_solver.f (row, 1, 0.0);
00504     m_solver.f (row, 2, 0.0);
00505 
00506     int ncps (0);
00507     for (int i = 0; i < id (n); i++)
00508       ncps += m_nurbs[i]->CVCount ();
00509 
00510     ON_EvaluateNurbsBasis (nurbs->Order (0), nurbs->m_knot[0] + E, params[n] (0), N0);
00511     ON_EvaluateNurbsBasis (nurbs->Order (1), nurbs->m_knot[1] + F, params[n] (1), N1);
00512 
00513     double s (1.0);
00514     n == 0 ? s = 1.0 : s = -1.0;
00515 
00516     for (int i = 0; i < nurbs->Order (0); i++)
00517     {
00518 
00519       for (int j = 0; j < nurbs->Order (1); j++)
00520       {
00521 
00522         m_solver.K (row, ncps + lrc2gl (*nurbs, E, F, i, j), weight * N0[i] * N1[j] * s);
00523 
00524       } // j
00525     } // i
00526 
00527     delete [] N1;
00528     delete [] N0;
00529   }
00530 
00531   row++;
00532 
00533 //  if (!m_quiet)
00534 //    printf ("[GlobalOptimization::addParamConstraint] row: %d / %d\n", row, m_nrows);
00535 }
00536 
00537 void
00538 GlobalOptimization::addPointConstraint (unsigned id, int ncps, const Eigen::Vector2d &params,
00539                                         const Eigen::Vector3d &point, double weight, unsigned &row)
00540 {
00541   ON_NurbsSurface *nurbs = m_nurbs[id];
00542 
00543   double *N0 = new double[nurbs->Order (0) * nurbs->Order (0)];
00544   double *N1 = new double[nurbs->Order (1) * nurbs->Order (1)];
00545 
00546   int E = ON_NurbsSpanIndex (nurbs->Order (0), nurbs->CVCount (0), nurbs->m_knot[0], params (0), 0, 0);
00547   int F = ON_NurbsSpanIndex (nurbs->Order (1), nurbs->CVCount (1), nurbs->m_knot[1], params (1), 0, 0);
00548 
00549   ON_EvaluateNurbsBasis (nurbs->Order (0), nurbs->m_knot[0] + E, params (0), N0);
00550   ON_EvaluateNurbsBasis (nurbs->Order (1), nurbs->m_knot[1] + F, params (1), N1);
00551 
00552   m_solver.f (row, 0, point (0) * weight);
00553   m_solver.f (row, 1, point (1) * weight);
00554   m_solver.f (row, 2, point (2) * weight);
00555 
00556   for (int i = 0; i < nurbs->Order (0); i++)
00557   {
00558 
00559     for (int j = 0; j < nurbs->Order (1); j++)
00560     {
00561 
00562       m_solver.K (row, ncps + lrc2gl (*nurbs, E, F, i, j), weight * N0[i] * N1[j]);
00563 
00564     } // j
00565 
00566   } // i
00567 
00568   row++;
00569 
00570   //  if (!m_quiet && !(row % 100))
00571   //    printf("[GlobalOptimization::addPointConstraint] row: %d / %d\n", row, m_nrows);
00572 
00573   delete [] N1;
00574   delete [] N0;
00575 }
00576 
00577 void
00578 GlobalOptimization::addCageInteriorRegularisation (unsigned id, int ncps, double weight, unsigned &row)
00579 {
00580   ON_NurbsSurface *nurbs = m_nurbs[id];
00581 
00582   for (int i = 1; i < (nurbs->CVCount (0) - 1); i++)
00583   {
00584     for (int j = 1; j < (nurbs->CVCount (1) - 1); j++)
00585     {
00586 
00587       m_solver.f (row, 0, 0.0);
00588       m_solver.f (row, 1, 0.0);
00589       m_solver.f (row, 2, 0.0);
00590 
00591       m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j + 0), -4.0 * weight);
00592       m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j - 1), 1.0 * weight);
00593       m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j + 1), 1.0 * weight);
00594       m_solver.K (row, ncps + grc2gl (*nurbs, i - 1, j + 0), 1.0 * weight);
00595       m_solver.K (row, ncps + grc2gl (*nurbs, i + 1, j + 0), 1.0 * weight);
00596 
00597       row++;
00598     }
00599   }
00600   //  if (!m_quiet && !(row % 100))
00601   //    printf("[GlobalOptimization::addCageInteriorRegularisation] row: %d / %d\n", row, m_nrows);
00602 }
00603 
00604 void
00605 GlobalOptimization::addCageBoundaryRegularisation (unsigned id, int ncps, double weight, int side, unsigned &row)
00606 {
00607   ON_NurbsSurface *nurbs = m_nurbs[id];
00608 
00609   int i = 0;
00610   int j = 0;
00611 
00612   switch (side)
00613   {
00614     case SOUTH:
00615       j = nurbs->CVCount (1) - 1;
00616     case NORTH:
00617       for (i = 1; i < (nurbs->CVCount (0) - 1); i++)
00618       {
00619 
00620         m_solver.f (row, 0, 0.0);
00621         m_solver.f (row, 1, 0.0);
00622         m_solver.f (row, 2, 0.0);
00623 
00624         m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j), -2.0 * weight);
00625         m_solver.K (row, ncps + grc2gl (*nurbs, i - 1, j), 1.0 * weight);
00626         m_solver.K (row, ncps + grc2gl (*nurbs, i + 1, j), 1.0 * weight);
00627 
00628         row++;
00629       }
00630       break;
00631 
00632     case EAST:
00633       i = nurbs->CVCount (0) - 1;
00634     case WEST:
00635       for (j = 1; j < (nurbs->CVCount (1) - 1); j++)
00636       {
00637 
00638         m_solver.f (row, 0, 0.0);
00639         m_solver.f (row, 1, 0.0);
00640         m_solver.f (row, 2, 0.0);
00641 
00642         m_solver.K (row, ncps + grc2gl (*nurbs, i, j + 0), -2.0 * weight);
00643         m_solver.K (row, ncps + grc2gl (*nurbs, i, j - 1), 1.0 * weight);
00644         m_solver.K (row, ncps + grc2gl (*nurbs, i, j + 1), 1.0 * weight);
00645 
00646         row++;
00647       }
00648       break;
00649   }
00650   //  if (!m_quiet && !(row % 100))
00651   //    printf("[GlobalOptimization::addCageBoundaryRegularisation] row: %d / %d\n", row, m_nrows);
00652 }
00653 
00654 void
00655 GlobalOptimization::addCageCornerRegularisation (unsigned id, int ncps, double weight, unsigned &row)
00656 {
00657   ON_NurbsSurface *nurbs = m_nurbs[id];
00658 
00659   { // NORTH-WEST
00660     int i = 0;
00661     int j = 0;
00662 
00663     m_solver.f (row, 0, 0.0);
00664     m_solver.f (row, 1, 0.0);
00665     m_solver.f (row, 2, 0.0);
00666 
00667     m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j + 0), -2.0 * weight);
00668     m_solver.K (row, ncps + grc2gl (*nurbs, i + 1, j + 0), 1.0 * weight);
00669     m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j + 1), 1.0 * weight);
00670 
00671     row++;
00672   }
00673 
00674   { // NORTH-EAST
00675     int i = nurbs->CVCount (0) - 1;
00676     int j = 0;
00677 
00678     m_solver.f (row, 0, 0.0);
00679     m_solver.f (row, 1, 0.0);
00680     m_solver.f (row, 2, 0.0);
00681 
00682     m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j + 0), -2.0 * weight);
00683     m_solver.K (row, ncps + grc2gl (*nurbs, i - 1, j + 0), 1.0 * weight);
00684     m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j + 1), 1.0 * weight);
00685 
00686     row++;
00687   }
00688 
00689   { // SOUTH-EAST
00690     int i = nurbs->CVCount (0) - 1;
00691     int j = nurbs->CVCount (1) - 1;
00692 
00693     m_solver.f (row, 0, 0.0);
00694     m_solver.f (row, 1, 0.0);
00695     m_solver.f (row, 2, 0.0);
00696 
00697     m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j + 0), -2.0 * weight);
00698     m_solver.K (row, ncps + grc2gl (*nurbs, i - 1, j + 0), 1.0 * weight);
00699     m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j - 1), 1.0 * weight);
00700 
00701     row++;
00702   }
00703 
00704   { // SOUTH-WEST
00705     int i = 0;
00706     int j = nurbs->CVCount (1) - 1;
00707 
00708     m_solver.f (row, 0, 0.0);
00709     m_solver.f (row, 1, 0.0);
00710     m_solver.f (row, 2, 0.0);
00711 
00712     m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j + 0), -2.0 * weight);
00713     m_solver.K (row, ncps + grc2gl (*nurbs, i + 1, j + 0), 1.0 * weight);
00714     m_solver.K (row, ncps + grc2gl (*nurbs, i + 0, j - 1), 1.0 * weight);
00715 
00716     row++;
00717   }
00718 
00719   //  if (!m_quiet && !(row % 100))
00720   //    printf("[GlobalOptimization::addCageCornerRegularisation] row: %d / %d\n", row, m_nrows);
00721 }
00722 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:31