bspline.cpp
Go to the documentation of this file.
00001 // 
00002 // Software License Agreement (BSD License)
00003 // 
00004 //   Copyright (c) 2011, Shulei Zhu <schuleichu@gmail.com>
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 Shulei Zhu 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 // bspline.cpp --- 
00036 // File            : bspline.cpp
00037 // Created: Sa Jun 18 14:04:27 2011 (+0200)
00038 // Author: Shulei Zhu
00039 
00040 // Code:
00041 
00042 #include <cv.h>
00043 #include "ccd/bspline.h"
00044 double BSpline::basic(int i,
00045                       int k,
00046                       double t)
00047 {
00048   double value;
00049   if (k == 1)
00050     ((t >= knots[i]) && (t<knots[i+1]))? value = 1 : value = 0;
00051   else{
00052     if((knots[i+k-1] == knots[i]) && (knots[i+k] == knots[i+1])) value = 0;
00053     else if(knots[i+k-1] == knots[i]) value = (knots[i+k] -t )/(knots[i+k] -knots[i+1])*basic(i+1,k -1 , t);
00054     else if(knots[i+k] == knots[i+1]) value = (t - knots[i])/(knots[i+k-1] - knots[i]) * basic(i,k-1, t);
00055     else value = (t - knots[i])/(knots[i+k-1] - knots[i]) * basic(i,k-1, t) + (knots[i+k] -t )/(knots[i+k] -knots[i+1])*basic(i+1,k -1 , t);
00056   }
00057   return value;
00058 }
00059 
00060 double BSpline::basic(int i,
00061                       int degree,
00062                       double t,
00063                       double *bp)
00064 {
00065   double temp = 0;
00066   double t_floor = t-floor(t);
00067   // std::cout << " t_floor: " << t_floor<<std::endl;
00068   if(degree == 3)
00069   {
00070     if(t -i >= 2 && t- i <= 3)
00071     {
00072       temp = 0.5*(1- t_floor)*(1-t_floor);
00073       *bp = t_floor - 1;
00074     }
00075     else if (t - i >= 1 && t-i< 2)
00076     {
00077       temp = -t_floor*t_floor + t_floor + 0.5;
00078       *bp = 1 - 2*t_floor;
00079     }
00080     else if (t-i >= 0 && t-i < 1)
00081     {
00082       temp = 0.5*t_floor*t_floor;
00083       *bp = t_floor;
00084     }
00085     else
00086     {
00087       temp = 0;
00088       *bp = 0;
00089     }
00090   }
00091   else if(degree == 4)
00092   {
00093     
00094     if((t -i >= 3 && t- i <= 4))
00095     {
00096       temp = (-t_floor*t_floor*t_floor+3*t_floor*t_floor - 3*t_floor + 1)/6;
00097       *bp = -0.5*t_floor*t_floor + t_floor - 0.5;
00098     }
00099     else if (t - i >= 2 && t-i< 3)
00100     {
00101       temp = (3*t_floor*t_floor*t_floor - 6*t_floor*t_floor + 4)/6;
00102       *bp = 1.5*t_floor*t_floor - 2*t_floor;
00103     }
00104     else if (t-i >= 1 && t-i < 2)
00105     {
00106       temp = (-3*t_floor*t_floor*t_floor + 3*t_floor*t_floor + 3*t_floor +1)/6;
00107       *bp = -1.5*t_floor*t_floor + t_floor + 0.5;
00108     }
00109     else if(t-i >= 0 && t-i < 1)
00110     {
00111       temp = t_floor*t_floor*t_floor/6;
00112       *bp = 0.5*t_floor*t_floor;
00113     }
00114     else
00115     {
00116       temp = 0;
00117       *bp = 0;
00118     }    
00119   }
00120   return temp;
00121 }
00122 
00123 
00124 void BSpline::computeKnots()
00125 {
00126     
00127   for (size_t j = 0; j < knots.size() ; ++j){
00128     knots[j] = j;
00129     // if (j < n_order)
00130     //     knots[j] = 0;
00131     // else if ((j >= n_order) && (j <= n_control_points))
00132     //     knots[j] = j-n_order + 1;
00133     // else if (j > n_control_points)
00134     //     knots[j] = n_control_points - n_order + 2;
00135     // std::cout << knots[j] << std::endl;
00136   }
00137 }
00138 
00139 void BSpline::computePoint(
00140     std::vector<cv::Point3d> control,
00141     cv::Point3d *output,
00142     cv::Point3d *slope,
00143     double *mat_ptr,
00144     double t,
00145     int degree)
00146 {
00147   double b = 0, bp = 0;
00148   // initialize the variables that will hold our outputted point
00149   output->x=0;
00150   output->y=0;
00151   output->z=0;
00152   slope->x = 0;
00153   slope->y = 0;
00154   slope->z = 0;
00155   for (size_t i = 0; i < control.size(); i++)
00156   {
00157     // b = basic(i, n_order_, t);
00158     b = basic(i, degree, t, &bp);
00159     mat_ptr[i] = b;
00160     output->x += control[i].x * b;
00161     output->y += control[i].y * b;
00162     output->z += control[i].z * b;
00163     slope->x += (control[i]).x * bp;
00164     slope->y += (control[i]).y * bp;
00165     slope->z += (control[i]).z * bp;
00166   }
00167 }  
00168 BSpline::BSpline():curve_(NULL), tangent_(NULL){}
00169 
00170 BSpline::BSpline(int n,
00171                  int resolution,
00172                  std::vector<cv::Point3d> control)
00173     :basic_mat_(cv::Mat(resolution, control.size(), CV_64FC1)),
00174      knots(std::vector<int>(control.size()+n, 0)),
00175   curve_((n>0 && resolution > 0)? new cv::Point3d[resolution]:NULL),
00176   tangent_((n>0 && resolution > 0)? new cv::Point3d[resolution]:NULL)
00177 {
00178   double increment, interval;
00179   cv::Point3d tmp_point, tmp_tangent;
00180   int m = control.size() - 1;
00181   computeKnots();
00182   increment = (double) (m - n + 1)/resolution;
00183   // interval = 0;
00184   // std::cout <<  "increment << " << increment << std::endl;  
00185   // for (interval = n-1; fabs(interval - m) > 0.0000001 ; ++i){
00186   interval = n -1;
00187   for (int i = 0; i < resolution; ++i){
00188     double *mat_ptr = basic_mat_.ptr<double>(i);
00189     computePoint(control, &tmp_point, &tmp_tangent, mat_ptr, interval, n);
00190     curve_[i].x = tmp_point.x;
00191     curve_[i].y = tmp_point.y;
00192     curve_[i].z = tmp_point.z;
00193     // if(i<20)
00194     //   std::cout << interval <<"i: " <<i << " x " << round(tmp_point.x) << " y "<< round(tmp_point.y) << std::endl;
00195     // std::cout <<  interval << "       i: " << i << " x " << curve_[i].x << " y "<< curve_[i].y << std::endl;
00196     tangent_[i].x = tmp_tangent.x;
00197     tangent_[i].y = tmp_tangent.y;
00198     tangent_[i].z = tmp_tangent.z;
00199     // double min = (double)(100%((int)round(increment*100)))/100.0;
00200     // std::cout <<"increment: " << (int)round(increment*100) <<  " min = " << min << " interval - round(interval) : " << abs(interval - round(interval))<< std::endl;
00201 
00202     // if(abs(interval - round(interval)) <= min )
00203     // {
00204     //   std::cout << "interval: " << interval << std::endl;
00205     // }
00206 
00207     interval += increment;
00208   }
00209   // std::cout <<"resolution: " << resolution<<  " i = " << i<<  std::endl;
00210   // curve_[resolution-1].x=control[m].x;
00211   // curve_[resolution-1].y=control[m].y;
00212 }
00213 
00214 
00215 BSpline::~BSpline(){
00216        /* basic_mat_.release(); */
00217        /* knots.clear(); */
00218        if (curve_ != NULL) delete [] curve_;
00219        if (tangent_ != NULL) delete [] tangent_;
00220   }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


contracting_curve_density_algorithm
Author(s): Shulei Zhu, Dejan Pangercic
autogenerated on Thu May 23 2013 14:41:44