[37] | 1 | #include <math.h> |
---|
| 2 | #include <string> |
---|
| 3 | #include <vector> |
---|
| 4 | #include <float.h> |
---|
| 5 | #include <algorithm> |
---|
| 6 | #ifndef _WIN32 |
---|
| 7 | #include <ext/algorithm> |
---|
| 8 | #endif |
---|
| 9 | #include <iostream> |
---|
| 10 | |
---|
| 11 | #include "Ops.h" |
---|
| 12 | |
---|
| 13 | //#define DEBUG_RANSAC |
---|
| 14 | |
---|
| 15 | using namespace MatVec; |
---|
| 16 | using namespace std; |
---|
| 17 | using namespace Geometry; |
---|
| 18 | using namespace MultiViewGeom; |
---|
| 19 | typedef vector<pair<Point2D<double>, Point2D<double> > > MatchVec2Im; |
---|
| 20 | |
---|
| 21 | namespace HomogConsistF { |
---|
| 22 | |
---|
| 23 | // Linear version for use in RANSAC |
---|
| 24 | template <class T, class MatchT> |
---|
| 25 | bool Linear(Matrix<T> &H, MatchT &Matches, const MultiViewGeom::FMatrix<T> &FM) { |
---|
| 26 | Matrix<double> EigenVecs(3,3), FT(3,3); |
---|
| 27 | // Do FM^{T}*FM |
---|
| 28 | double sum; |
---|
| 29 | // TransposeT() |
---|
| 30 | for (unsigned int i=0;i<3;i++) |
---|
| 31 | for (unsigned int k=0;k<3;k++) |
---|
| 32 | { |
---|
| 33 | sum=FM(i,0)*FM(k,0); |
---|
| 34 | for (unsigned int j=1;j<3;j++) |
---|
| 35 | sum+=FM(i,j)*FM(k,j); |
---|
| 36 | |
---|
| 37 | FT(i,k)=sum; |
---|
| 38 | } |
---|
| 39 | double EigenVals[3]; |
---|
| 40 | MatrixOps::EigenValVec_Symmetric(FT, EigenVals, EigenVecs); |
---|
| 41 | Homg2DPoint<double> e2(EigenVecs(0,0), EigenVecs(1,0), EigenVecs(2,0)); |
---|
| 42 | double norm=1.0/sqrt(e2.x()+e2.y()+e2.w()); |
---|
| 43 | e2.x()*=norm; e2.y()*=norm; e2.w()*=norm; |
---|
| 44 | |
---|
| 45 | const T e2x=e2.x(), e2y=e2.y(), e2w=e2.w(); |
---|
| 46 | // Helps numerical stability in some near degenerate cases to at least have a value in a1,a2,a3 |
---|
| 47 | T a1=0.3, a2=0.3, a3=0.3; |
---|
| 48 | H(0,0)=e2y*FM(2,0)-e2w*FM(1,0)+e2x*a1; |
---|
| 49 | H(0,1)=e2y*FM(2,1)-e2w*FM(1,1)+e2x*a2; |
---|
| 50 | H(0,2)=e2y*FM(2,2)-e2w*FM(1,2)+e2x*a3; |
---|
| 51 | |
---|
| 52 | H(1,0)=e2w*FM(0,0)-e2x*FM(2,0)+e2y*a1; |
---|
| 53 | H(1,1)=e2w*FM(0,1)-e2x*FM(2,1)+e2y*a2; |
---|
| 54 | H(1,2)=e2w*FM(0,2)-e2x*FM(2,2)+e2y*a3; |
---|
| 55 | |
---|
| 56 | H(2,0)=e2x*FM(1,0)-e2y*FM(0,0)+e2w*a1; |
---|
| 57 | H(2,1)=e2x*FM(1,1)-e2y*FM(0,1)+e2w*a2; |
---|
| 58 | H(2,2)=e2x*FM(1,2)-e2y*FM(0,2)+e2w*a3; |
---|
| 59 | |
---|
| 60 | // Build a constraint matrix |
---|
| 61 | Matrix<T> A(Matches.size(), 3); |
---|
| 62 | double *b=new double[Matches.size()]; |
---|
| 63 | |
---|
| 64 | for (unsigned int i=0; i<Matches.size(); ++i) { |
---|
| 65 | const T ux=Matches[i].first.x(), uy=Matches[i].first.y(); |
---|
| 66 | const T u2x=Matches[i].second.x(), u2y=Matches[i].second.y(); |
---|
| 67 | const T u2w=1.0; |
---|
| 68 | |
---|
| 69 | const T theta2=-atan((u2w*e2y-e2w*u2y)/(u2w*e2x-e2w*u2x)); |
---|
| 70 | |
---|
| 71 | // Constraints on x |
---|
| 72 | A(i,0)=(-cos(theta2)*u2w*e2x+sin(theta2)*u2w*e2y-(-cos(theta2)*u2x+sin(theta2)*u2y)*e2w)*ux; |
---|
| 73 | A(i,1)=(-cos(theta2)*u2w*e2x+sin(theta2)*u2w*e2y-(-cos(theta2)*u2x+sin(theta2)*u2y)*e2w)*uy; |
---|
| 74 | A(i,2)=(-cos(theta2)*u2w*e2x+sin(theta2)*u2w*e2y-(-cos(theta2)*u2x+sin(theta2)*u2y)*e2w); |
---|
| 75 | b[i]=-((cos(theta2)*u2w*H(0,0)-sin(theta2)*u2w*H(1,0)+(-cos(theta2)*u2x+sin(theta2)*u2y)*H(2,0))*ux+(-cos(theta2)*u2x+sin(theta2)*u2y)*H(2,2)+(cos(theta2)*u2w*H(0,1)-sin(theta2)*u2w*H(1,1)+(-cos(theta2)*u2x+sin(theta2)*u2y)*H(2,1))*uy+cos(theta2)*u2w*H(0,2)-sin(theta2)*u2w*H(1,2)); |
---|
| 76 | |
---|
| 77 | } |
---|
| 78 | |
---|
| 79 | double x[3]; |
---|
| 80 | bool ok=LAPACK::LeastSquaresRankDeff(A,b,x); |
---|
| 81 | |
---|
| 82 | delete[] b; |
---|
| 83 | |
---|
| 84 | a1=x[0]; a2=x[1]; a3=x[2]; |
---|
| 85 | |
---|
| 86 | H(0,0)=H(0,0)-e2x*a1; |
---|
| 87 | H(0,1)=H(0,1)-e2x*a2; |
---|
| 88 | H(0,2)=H(0,2)-e2x*a3; |
---|
| 89 | |
---|
| 90 | H(1,0)=H(1,0)-e2y*a1; |
---|
| 91 | H(1,1)=H(1,1)-e2y*a2; |
---|
| 92 | H(1,2)=H(1,2)-e2y*a3; |
---|
| 93 | |
---|
| 94 | H(2,0)=H(2,0)-e2w*a1; |
---|
| 95 | H(2,1)=H(2,1)-e2w*a2; |
---|
| 96 | H(2,2)=H(2,2)-e2w*a3; |
---|
| 97 | |
---|
| 98 | return ok; |
---|
| 99 | } |
---|
| 100 | |
---|
| 101 | |
---|
| 102 | |
---|
| 103 | |
---|
| 104 | |
---|
| 105 | |
---|
| 106 | |
---|
| 107 | |
---|
| 108 | |
---|
| 109 | |
---|
| 110 | |
---|
| 111 | |
---|
| 112 | double GetResidual_RANSAC(const pair<Point2D<double>, Point2D<double> > &Match, |
---|
| 113 | const Matrix<double> &H) { |
---|
| 114 | |
---|
| 115 | const double px=Match.first[0], py=Match.first[1], pw=1.0; |
---|
| 116 | double tx=H(0,0)*px+H(0,1)*py+H(0,2)*pw; |
---|
| 117 | double ty=H(1,0)*px+H(1,1)*py+H(1,2)*pw; |
---|
| 118 | double tz=H(2,0)*px+H(2,1)*py+H(2,2)*pw; |
---|
| 119 | Point2D<double> NP(tx/tz,ty/tz); |
---|
| 120 | double xdiff=NP.x()-Match.second.x(), ydiff=NP.y()-Match.second.y(); |
---|
| 121 | return xdiff*xdiff+ydiff*ydiff; |
---|
| 122 | } |
---|
| 123 | |
---|
| 124 | // The function itself |
---|
| 125 | void RANSAC(const vector<pair<Point2D<double>, Point2D<double> > > &Matches, |
---|
| 126 | vector<pair<Point2D<double>, Point2D<double> > > &OutlierFree, |
---|
| 127 | Matrix<double> &ReturnH, const FMatrix<double> &FM) { |
---|
| 128 | |
---|
| 129 | // Copy out of the iterator and into a vector so can normalise without destroying callers copy. |
---|
| 130 | unsigned int i; |
---|
| 131 | vector<Matrix<double> > Hs; |
---|
| 132 | // I know - I know - this is just in this simplified version |
---|
| 133 | OutlierFree.clear(); |
---|
| 134 | |
---|
| 135 | vector<pair<Point2D<double>, Point2D<double> > >::size_type numm=Matches.size(); |
---|
| 136 | |
---|
| 137 | if (numm<10) { |
---|
| 138 | cerr << "Planar homography RANSAC estimate (consistant with fundamental matrix) must be given at least 10 point matches to work!. So, go away and don't come back until you've got some more!\n" << endl; |
---|
| 139 | exit(1); |
---|
| 140 | } |
---|
| 141 | |
---|
| 142 | unsigned int minsample=10; |
---|
| 143 | // LMedS style calculation of number of sub-samples |
---|
| 144 | unsigned int NumberSubSamples=400; |
---|
| 145 | |
---|
| 146 | #ifdef DEBUG_RANSAC |
---|
| 147 | cerr << "Calculating Planar Homography matrix using LMedS:" << endl; |
---|
| 148 | cerr << "Number matches=" << numm << endl; |
---|
| 149 | cerr << "Using " << NumberSubSamples << " 4 point subsamples" << endl; |
---|
| 150 | #endif |
---|
| 151 | |
---|
| 152 | // Pre-allocate match vector for the four point algorithm |
---|
| 153 | vector<pair<Point2D<double>, Point2D<double> > > PassThrough(minsample); |
---|
| 154 | |
---|
| 155 | // So, run the 3 point algorithm on NumberSubSamples subsamples |
---|
| 156 | for (i=0;i<NumberSubSamples;i++) |
---|
| 157 | { |
---|
| 158 | // They always insist on removing the most useful functions in the windows STL. B******S |
---|
| 159 | #ifndef _WIN32 |
---|
| 160 | // Randomly select minsample points |
---|
| 161 | random_sample_n(Matches.begin(), Matches.end(), PassThrough.begin(), minsample); |
---|
| 162 | #else |
---|
| 163 | // This version is a bit naff since duplicates can occur |
---|
| 164 | unsigned int picked=0; |
---|
| 165 | while (picked<minsample) { |
---|
| 166 | int off=rand()%(int)numm; |
---|
| 167 | PassThrough[picked++]=Matches[off]; |
---|
| 168 | } |
---|
| 169 | #endif |
---|
| 170 | |
---|
| 171 | // Run the minimal algorithm |
---|
| 172 | Matrix<double> H(3,3); |
---|
| 173 | if (Linear(H, PassThrough, FM)) |
---|
| 174 | Hs.push_back(H); |
---|
| 175 | } |
---|
| 176 | |
---|
| 177 | // Now, need to determine the median of the squared residuals, for each image |
---|
| 178 | double *residuals=new double[numm], *bestr=new double[numm]; |
---|
| 179 | double minhuber=DBL_MAX, bestmaxerr=0.0; |
---|
| 180 | double med; |
---|
| 181 | int residual; |
---|
| 182 | vector<Matrix<double> >::const_iterator Hs_iter; |
---|
| 183 | MatchVec2Im::const_iterator iter; |
---|
| 184 | Matrix<double> BestH(3,3); |
---|
| 185 | |
---|
| 186 | for (Hs_iter=Hs.begin(); Hs_iter!=Hs.end(); ++Hs_iter) |
---|
| 187 | { |
---|
| 188 | const Matrix<double> &H=*Hs_iter; |
---|
| 189 | residual=0; |
---|
| 190 | |
---|
| 191 | // First run through all matches and evaluate the residual |
---|
| 192 | for (iter=Matches.begin();iter!=Matches.end();++iter) |
---|
| 193 | // evaluate d(m, Hm')^2 + d(m', H^T m)^2 |
---|
| 194 | residuals[residual++]=GetResidual_RANSAC(*iter, H); |
---|
| 195 | |
---|
| 196 | // Now, find the median of the results. |
---|
| 197 | if (residual>0) |
---|
| 198 | { |
---|
| 199 | med=MiscMath::VectorMedian(residuals, residuals+residual); |
---|
| 200 | |
---|
| 201 | // Get confidence limit for Huber function |
---|
| 202 | double robuststddev=1.4826*(1.0+(5.0/(numm-minsample)))*sqrt(med); |
---|
| 203 | double maxerr=3.84*robuststddev*robuststddev; |
---|
| 204 | |
---|
| 205 | // Now, go through residuals and sum |
---|
| 206 | double hubersum=0.0; |
---|
| 207 | for (int i=0;i<residual;++i) { |
---|
| 208 | if (residuals[i]<maxerr) |
---|
| 209 | hubersum+=residuals[i]; |
---|
| 210 | else |
---|
| 211 | hubersum+=maxerr; |
---|
| 212 | } |
---|
| 213 | |
---|
| 214 | if (hubersum<minhuber) |
---|
| 215 | { |
---|
| 216 | BestH=H; minhuber=hubersum; bestmaxerr=maxerr; |
---|
| 217 | copy(residuals, residuals+residual, bestr); |
---|
| 218 | } |
---|
| 219 | } |
---|
| 220 | } |
---|
| 221 | Hs.clear(); |
---|
| 222 | |
---|
| 223 | // Now, discard all false matches |
---|
| 224 | #ifdef DEBUG_RANSAC |
---|
| 225 | cerr << "Outlier threshold (95%) maxerr=" << bestmaxerr << endl; |
---|
| 226 | #endif |
---|
| 227 | |
---|
| 228 | // Work through all matches and discard if residual squared is less than just |
---|
| 229 | // calculated robust value. |
---|
| 230 | unsigned int off, outliers=0; |
---|
| 231 | for (off=0;off<numm;++off) |
---|
| 232 | { |
---|
| 233 | if (bestr[off]>bestmaxerr) |
---|
| 234 | ++outliers; |
---|
| 235 | else |
---|
| 236 | OutlierFree.push_back(Matches[off]); |
---|
| 237 | } |
---|
| 238 | |
---|
| 239 | ReturnH=BestH; |
---|
| 240 | |
---|
| 241 | #ifdef DEBUG_RANSAC |
---|
| 242 | cerr << endl << "Completed - found " << outliers << " outliers in " << numm << " points" << endl; |
---|
| 243 | #endif |
---|
| 244 | |
---|
| 245 | delete[] residuals; |
---|
| 246 | delete[] bestr; |
---|
| 247 | } |
---|
| 248 | |
---|
| 249 | #undef DEBUG_RANSAC |
---|
| 250 | |
---|
| 251 | } |
---|
| 252 | |
---|
| 253 | namespace MatrixOps { |
---|
| 254 | |
---|
| 255 | extern "C" { |
---|
| 256 | void dspevd_(char *, char *, int *, double *, double *, double *, int *, double *, int *, int *, int *, int *); |
---|
| 257 | } |
---|
| 258 | |
---|
| 259 | /** |
---|
| 260 | Finds the eigenvalues and eigenvectors of a symmetric matrix using LAPACK routines. Returns eigenvalues and eigenvectors in ascending order (lowest first). |
---|
| 261 | {\bf Errors:} |
---|
| 262 | \begin{itemize} |
---|
| 263 | \item {\em 1:} Error in EigenValVec_Symmetric: Supplied matrix is not symmetric - This is returned whenever the supplied matrix is not symmetric. |
---|
| 264 | \item {\em 2:} Error in EigenValVec_Symmetric: Eigenvalue and Eigenvector arrays are not the correct size - If the returning eigenvalues/eigenvector arrays are incorrectly sized this is returned.. |
---|
| 265 | */ |
---|
| 266 | void EigenValVec_Symmetric(const Matrix<double> &A, double *EigenVals, Matrix<double> &EigenVecs) |
---|
| 267 | { |
---|
| 268 | // First, check if it is symmetric |
---|
| 269 | unsigned int i,j; |
---|
| 270 | size_t N=A.num_rows(); |
---|
| 271 | bool Symm=true; |
---|
| 272 | |
---|
| 273 | if (N!=A.num_cols()) |
---|
| 274 | Symm=false; |
---|
| 275 | else |
---|
| 276 | { |
---|
| 277 | for (i=0;i<N-1;i++) |
---|
| 278 | for (j=0;j<i;j++) |
---|
| 279 | if (A(i,j)!=A(j,i)) |
---|
| 280 | Symm=false; |
---|
| 281 | } |
---|
| 282 | |
---|
| 283 | if (Symm==false) { |
---|
| 284 | throw 1; |
---|
| 285 | } |
---|
| 286 | |
---|
| 287 | double *A_Packed=new double[(N*(N+1))/2]; |
---|
| 288 | unsigned int indx=0; |
---|
| 289 | // Put into packed storage |
---|
| 290 | for (i=0;i<N;i++) |
---|
| 291 | for (j=0;j<=i;j++) |
---|
| 292 | A_Packed[indx++]=A(i,j); |
---|
| 293 | |
---|
| 294 | int rinfo=0; |
---|
| 295 | int LWork=(int)(1+6*N+N*N); |
---|
| 296 | double *Work=new double[LWork]; |
---|
| 297 | int LIWork=(int)(3+5*N); |
---|
| 298 | int *IWork=new int[LIWork]; |
---|
| 299 | int N_=(int)N; // Has to be an int for passing to fortran code |
---|
| 300 | |
---|
| 301 | EigenVecs.resize(N,N,false); |
---|
| 302 | |
---|
| 303 | dspevd_("V", "U", &N_, A_Packed, &EigenVals[0], &EigenVecs(0,0), &N_, Work, &LWork, IWork, &LIWork, &rinfo); |
---|
| 304 | |
---|
| 305 | delete[] A_Packed; |
---|
| 306 | delete[] Work; |
---|
| 307 | delete[] IWork; |
---|
| 308 | |
---|
| 309 | assert(rinfo==0); |
---|
| 310 | return; |
---|
| 311 | |
---|
| 312 | } |
---|
| 313 | |
---|
| 314 | // The LAPACK library functions |
---|
| 315 | extern "C" { |
---|
| 316 | int dgetrf_(int *m, int *n, double *a, int *lda, int *ipiv, int *info); |
---|
| 317 | int dgetri_(int *n, double *a, int *lda, int *ipiv, double *work, int *lwork, int *info); |
---|
| 318 | } |
---|
| 319 | |
---|
| 320 | bool inverse(const MatVec::Matrix<double> &A, MatVec::Matrix<double> &AInv) { |
---|
| 321 | |
---|
| 322 | int N = (int)A.num_rows(); |
---|
| 323 | if (A.num_rows()!=A.num_cols() || N<1) |
---|
| 324 | return false; |
---|
| 325 | |
---|
| 326 | // Going to use AInv as our workspace |
---|
| 327 | AInv=A; |
---|
| 328 | |
---|
| 329 | // Set up storage |
---|
| 330 | int *ipiv = new int[N]; |
---|
| 331 | int info; |
---|
| 332 | |
---|
| 333 | // Call for the LU factorisation |
---|
| 334 | dgetrf_(&N, &N, &(AInv(0,0)), &N, ipiv, &info); |
---|
| 335 | |
---|
| 336 | if (info>0) { |
---|
| 337 | std::cerr << "info=" << info << std::endl; |
---|
| 338 | delete[] ipiv; |
---|
| 339 | return false; |
---|
| 340 | } |
---|
| 341 | |
---|
| 342 | // Set up workspace |
---|
| 343 | int lwork = N; |
---|
| 344 | double *workd = new double[lwork]; |
---|
| 345 | |
---|
| 346 | // Now, call for inverse |
---|
| 347 | dgetri_(&N, &(AInv(0,0)), &N, ipiv, workd, &lwork, &info); |
---|
| 348 | |
---|
| 349 | delete[] ipiv; |
---|
| 350 | delete[] workd; |
---|
| 351 | |
---|
| 352 | // If got a singular matrix |
---|
| 353 | if (info>0) |
---|
| 354 | return false; |
---|
| 355 | |
---|
| 356 | return true; |
---|
| 357 | } |
---|
| 358 | |
---|
| 359 | |
---|
| 360 | } |
---|
| 361 | |
---|
| 362 | |
---|
| 363 | |
---|
| 364 | |
---|
| 365 | namespace MiscMath { |
---|
| 366 | |
---|
| 367 | double selectnth_heap(unsigned long m, double *ArrBegin, double *ArrEnd) { |
---|
| 368 | typedef double DataT; |
---|
| 369 | |
---|
| 370 | unsigned long i,j,k,n=(unsigned long)(distance(ArrBegin, ArrEnd)); |
---|
| 371 | DataT swap; |
---|
| 372 | m=n-m+1; // Since want to find nth lowest value, not nth highest value |
---|
| 373 | |
---|
| 374 | // const DataT *arr=ArrBegin-1; |
---|
| 375 | DataT *heap=new DataT[m+2]; |
---|
| 376 | |
---|
| 377 | copy(ArrBegin, ArrBegin+m, heap); |
---|
| 378 | sort(heap, heap+m); |
---|
| 379 | --heap; |
---|
| 380 | |
---|
| 381 | for (i=m+1;i<=n;++i) { |
---|
| 382 | if (ArrBegin[i-1]>heap[1]) { |
---|
| 383 | heap[1]=ArrBegin[i-1]; |
---|
| 384 | for (j=1;;) { |
---|
| 385 | k=j << 1; |
---|
| 386 | if (k>m) break; |
---|
| 387 | if (k!=m && heap[k] > heap[k+1]) ++k; |
---|
| 388 | if (heap[j] <= heap[k]) break; |
---|
| 389 | swap=heap[k]; |
---|
| 390 | heap[k]=heap[j]; |
---|
| 391 | heap[j]=swap; |
---|
| 392 | j=k; |
---|
| 393 | } |
---|
| 394 | } |
---|
| 395 | } |
---|
| 396 | |
---|
| 397 | DataT rval=heap[1]; |
---|
| 398 | ++heap; |
---|
| 399 | delete[] heap; |
---|
| 400 | |
---|
| 401 | return rval; |
---|
| 402 | } |
---|
| 403 | |
---|
| 404 | double VectorMedian(double *VecBegin, double *VecEnd) { |
---|
| 405 | if (VecBegin==VecEnd) |
---|
| 406 | return 0.0; |
---|
| 407 | |
---|
| 408 | double median; |
---|
| 409 | |
---|
| 410 | unsigned int size=(unsigned int)(distance(VecBegin, VecEnd)); |
---|
| 411 | |
---|
| 412 | if (size % 2 == 0) |
---|
| 413 | { |
---|
| 414 | // Don't use pedants if enough items in the iterator to get away with it |
---|
| 415 | if (size>100) |
---|
| 416 | median=MiscMath::selectnth_heap(size/2, VecBegin, VecEnd); |
---|
| 417 | else |
---|
| 418 | median= |
---|
| 419 | (MiscMath::selectnth_heap(size/2, VecBegin, VecEnd)+ |
---|
| 420 | MiscMath::selectnth_heap((size/2)+1, VecBegin, VecEnd))/2.0; |
---|
| 421 | } |
---|
| 422 | else |
---|
| 423 | // Odd case |
---|
| 424 | median=MiscMath::selectnth_heap(((size+1)/2)-1, VecBegin, VecEnd); |
---|
| 425 | |
---|
| 426 | return median; |
---|
| 427 | } |
---|
| 428 | |
---|
| 429 | } |
---|
| 430 | |
---|
| 431 | namespace LAPACK { |
---|
| 432 | |
---|
| 433 | extern "C" { |
---|
| 434 | int dgelsy_(int *, int *, int *, double *, int *, double *, int *, int *, double *, int *, double *, int *, int *); |
---|
| 435 | } |
---|
| 436 | |
---|
| 437 | bool LeastSquaresRankDeff(Matrix<double>& A, const double *B, double *X) { |
---|
| 438 | int m=(int)A.num_rows(), n=(int)A.num_cols(); |
---|
| 439 | int info=1, one=1; |
---|
| 440 | int *jpvt=new int[n]; |
---|
| 441 | int rank; |
---|
| 442 | double rcond=1e-14; |
---|
| 443 | |
---|
| 444 | int mn=m; |
---|
| 445 | if (n<m) |
---|
| 446 | mn=n; |
---|
| 447 | int lwork=mn+3*n+1; |
---|
| 448 | if (2*mn+1>lwork) |
---|
| 449 | lwork=2*mn+1; |
---|
| 450 | double *Work=new double[lwork]; |
---|
| 451 | |
---|
| 452 | // copy B into temp holder since X is wrong dimensions |
---|
| 453 | double *Temp=new double[m]; |
---|
| 454 | for (int i=0;i<m;++i) |
---|
| 455 | Temp[i]=B[i]; |
---|
| 456 | |
---|
| 457 | dgelsy_(&m, &n, &one, &(A(0,0)), &m, &Temp[0], &m, &jpvt[0], &rcond, &rank, &Work[0], &lwork, &info); |
---|
| 458 | |
---|
| 459 | if (info<0) { |
---|
| 460 | // cerr << "Error from linear least square solver (LAPACK routines)" << endl; |
---|
| 461 | return false; |
---|
| 462 | } |
---|
| 463 | |
---|
| 464 | for (int i=0;i<n;++i) |
---|
| 465 | X[i]=Temp[i]; |
---|
| 466 | |
---|
| 467 | delete[] jpvt; |
---|
| 468 | delete[] Work; |
---|
| 469 | delete[] Temp; |
---|
| 470 | return true; |
---|
| 471 | } |
---|
| 472 | |
---|
| 473 | } |
---|