PhotometricOptimizer.cpp

Go to the documentation of this file.
00001 // -*- c-basic-offset: 4 -*-
00024 #include "PhotometricOptimizer.h"
00025 
00026 #include <fstream>
00027 #include <foreign/levmar/lm.h>
00028 #include <photometric/ResponseTransform.h>
00029 
00030 #ifdef DEBUG
00031 #define DEBUG_LOG_VIG 1
00032 #endif
00033 
00034 
00035 namespace HuginBase {
00036     
00037 using namespace std;
00038 
00039 
00041 inline double weightHuber(double x, double sigma)
00042 {
00043     if (x > sigma) {
00044         x = sqrt(sigma* (2*x - sigma));
00045     }
00046     return x;
00047 }
00048 
00049 
00050 
00051 PhotometricOptimizer::OptimData::OptimData(const PanoramaData & pano, const OptimizeVector & optvars,
00052                                            const std::vector<vigra_ext::PointPairRGB> & data,
00053                                            double mEstimatorSigma, bool symmetric,
00054                                            int maxIter, AppBase::ProgressReporter & progress)
00055   : m_pano(pano), m_data(data), huberSigma(mEstimatorSigma), symmetricError(symmetric),
00056     m_maxIter(maxIter), m_progress(progress)
00057 {
00058     assert(pano.getNrOfImages() == optvars.size());
00059         
00060     for (unsigned i=0; i < pano.getNrOfImages(); i++) {
00061         m_imgs.push_back(pano.getSrcImage(i));
00062     }
00063 
00064     std::vector<std::set<std::string> > usedVars(pano.getNrOfImages());
00065 
00066     // create variable map with param <-> var assignments
00067     for (unsigned i=0; i < optvars.size(); i++) 
00068     {
00069         const std::set<std::string> vars = optvars[i];
00070         const SrcPanoImage & img_i = pano.getImage(i);
00071         for (std::set<std::string>::const_iterator it = vars.begin();
00072              it != vars.end(); ++it)
00073         {
00074             VarMapping var;
00075             var.type = *it;
00076             //check if variable is yet included
00077             if(set_contains(usedVars[i],var.type))
00078                 continue;
00079             var.imgs.insert(i);
00080             usedVars[i].insert(var.type);
00081             //now check all linked images and add image nr
00082 #define CheckLinked(name)\
00083             if(img_i.name##isLinked())\
00084             {\
00085                 for(unsigned j=i+1;j<pano.getNrOfImages();j++)\
00086                     if(img_i.name##isLinkedWith(pano.getImage(j)))\
00087                     {\
00088                         var.imgs.insert(j);\
00089                         usedVars[j].insert(var.type);\
00090                     };\
00091             }
00092 
00093             if(var.type=="Eev")
00094             {
00095                 CheckLinked(ExposureValue)
00096             };
00097             if(var.type=="Er")
00098             {
00099                 CheckLinked(WhiteBalanceRed)
00100             };
00101             if(var.type=="Eb")
00102             {
00103                 CheckLinked(WhiteBalanceBlue)
00104             };
00105             if(var.type[0]=='R')
00106             {
00107                 CheckLinked(EMoRParams)
00108             };
00109             if(var.type=="Va" || var.type=="Vb" || var.type=="Vc" || var.type=="Vd")
00110             {
00111                 CheckLinked(RadialVigCorrCoeff)
00112             }
00113             if(var.type=="Vx" || var.type=="Vy")
00114             {
00115                 CheckLinked(RadialVigCorrCenterShift)
00116             };
00117 #undef CheckLinked
00118             m_vars.push_back(var);
00119         }
00120     }
00121 }
00122 
00123 void PhotometricOptimizer::OptimData::ToX(double * x)
00124 {
00125     for (size_t i=0; i < m_vars.size(); i++)
00126     {
00127         assert(m_vars[i].imgs.size() > 0);
00128             // get corresponding image number
00129         unsigned j = *(m_vars[i].imgs.begin());
00130             // get value
00131         x[i] = m_imgs[j].getVar(m_vars[i].type);
00132         // TODO: transform some variables, such as the vignetting center!
00133     }
00134 }
00135 
00136 
00137 void PhotometricOptimizer::OptimData::FromX(double * x)
00138 {
00139     for (size_t i=0; i < m_vars.size(); i++)
00140     {
00141         // TODO: transform some variables, such as the vignetting center!
00142         assert(m_vars[i].imgs.size() > 0);
00143             // copy value int all images
00144         for (std::set<unsigned>::const_iterator it = m_vars[i].imgs.begin();
00145              it != m_vars[i].imgs.end(); ++it)
00146         {
00147             m_imgs[*it].setVar(m_vars[i].type, x[i]);
00148         }
00149     }
00150 }
00151 
00152 
00153 
00154 void PhotometricOptimizer::photometricError(double *p, double *x, int m, int n, void * data)
00155 {
00156 #ifdef DEBUG_LOG_VIG
00157     static int iter = 0;
00158 #endif
00159     typedef Photometric::ResponseTransform<vigra::RGBValue<double> > RespFunc;
00160     typedef Photometric::InvResponseTransform<vigra::RGBValue<double>, vigra::RGBValue<double> > InvRespFunc;
00161 
00162     int xi = 0 ;
00163 
00164     OptimData * dat = (OptimData *) data;
00165     dat->FromX(p);
00166 #ifdef DEBUG_LOG_VIG
00167     ostringstream oss;
00168     oss << "vig_log_" << iter;
00169     iter++;
00170     ofstream log(oss.str().c_str());
00171     log << "VIGparams = [";
00172     for (int i = 0; i < m; i++) {
00173         log << p[i] << " ";
00174     }
00175     log << " ]; " << std::endl;
00176     // TODO: print parameters of images.
00177     std::ofstream script("vig_test.pto");
00178     OptimizeVector optvars(dat->m_pano.getNrOfImages());
00179     UIntSet imgs = dat->m_pano.getActiveImages();
00180     dat->m_pano.printPanoramaScript(script, optvars, dat->m_pano.getOptions(), imgs, false, "");
00181 #endif
00182 
00183     size_t nImg = dat->m_imgs.size();
00184     std::vector<RespFunc> resp(nImg);
00185     std::vector<InvRespFunc> invResp(nImg);
00186     for (size_t i=0; i < nImg; i++) {
00187         resp[i] = RespFunc(dat->m_imgs[i]);
00188         invResp[i] = InvRespFunc(dat->m_imgs[i]);
00189         // calculate the monotonicity error
00190         double monErr = 0;
00191         if (dat->m_imgs[i].getResponseType() == SrcPanoImage::RESPONSE_EMOR) {
00192             // calculate monotonicity error
00193             int lutsize = resp[i].m_lutR.size();
00194             for (int j=0; j < lutsize-1; j++)
00195             {
00196                 double d = resp[i].m_lutR[j] - resp[i].m_lutR[j+1];
00197                 if (d > 0) {
00198                     monErr += d*d*lutsize;
00199                 }
00200             }
00201         }
00202         x[xi++] = monErr;
00203                 // enforce a montonous response curves
00204                 resp[i].enforceMonotonicity();
00205                 invResp[i].enforceMonotonicity();
00206     }
00207 
00208     double sqerror=0;
00209     // loop over all points to calculate the error
00210 #ifdef DEBUG_LOG_VIG
00211     log << "VIGval = [ ";
00212 #endif
00213 
00214     for (std::vector<vigra_ext::PointPairRGB>::const_iterator it = dat->m_data.begin();
00215          it != dat->m_data.end(); ++it)
00216     {
00217         vigra::RGBValue<double> l2 = invResp[it->imgNr2](it->i2, it->p2);
00218         vigra::RGBValue<double> i2ini1 = resp[it->imgNr1](l2, it->p1);
00219         vigra::RGBValue<double> error = it->i1 - i2ini1;
00220 
00221 
00222         // if requested, calcuate the error in image 2 as well.
00223         //TODO: weighting dependent on the pixel value? check if outside of i2 range?
00224         vigra::RGBValue<double> l1 = invResp[it->imgNr1](it->i1, it->p1);
00225         vigra::RGBValue<double> i1ini2 = resp[it->imgNr2](l1, it->p2);
00226         vigra::RGBValue<double> error2 = it->i2 - i1ini2;
00227 
00228         for (int i=0; i < 3; i++) {
00229             sqerror += error[i]*error[i];
00230             sqerror += error2[i]*error2[i];
00231         }
00232 
00233         // use huber robust estimator
00234         if (dat->huberSigma > 0) {
00235             for (int i=0; i < 3; i++) {
00236                 x[xi++] = weightHuber(fabs(error[i]), dat->huberSigma);
00237                 x[xi++] = weightHuber(fabs(error2[i]), dat->huberSigma);
00238             }
00239         } else {
00240             x[xi++] = error[0];
00241             x[xi++] = error[1];
00242             x[xi++] = error[2];
00243             x[xi++] = error2[0];
00244             x[xi++] = error2[1];
00245             x[xi++] = error2[2];
00246         }
00247 
00248 #ifdef DEBUG_LOG_VIG
00249         log << it->i1.green()  << " "<< l1.green()  << " " << i1ini2.green() << "   " 
00250              << it->i2.green()  << " "<< l2.green()  << " " << i2ini1.green() << ";  " << std::endl;
00251 #endif
00252 
00253     }
00254 #ifdef DEBUG_LOG_VIG
00255     log << std::endl << "VIGerr = [";
00256     for (int i = 0; i < n; i++) {
00257         log << x[i] << std::endl;
00258     }
00259     log << " ]; " << std::endl;
00260 #endif
00261 
00262     DEBUG_DEBUG("squared error: " << sqerror);
00263 }
00264 
00265 int PhotometricOptimizer::photometricVis(double *p, double *x, int m, int n, int iter, double sqerror, void * data)
00266 {
00267     OptimData * dat = (OptimData *) data;
00268     char tmp[200];
00269     tmp[199] = 0;
00270     double error = sqrt(sqerror/n)*255;
00271     snprintf(tmp,199, "Iteration: %d, error: %f", iter, error);
00272     return dat->m_progress.increaseProgress(0.0, tmp) ? 1 : 0 ;
00273 }
00274 
00275 void PhotometricOptimizer::optimizePhotometric(PanoramaData & pano, const OptimizeVector & vars,
00276                                                const std::vector<vigra_ext::PointPairRGB> & correspondences,
00277                                                AppBase::ProgressReporter & progress,
00278                                                double & error)
00279 {
00280 
00281     OptimizeVector photometricVars;
00282     // keep only the photometric variables
00283     unsigned int optCount=0;
00284     for (OptimizeVector::const_iterator it=vars.begin(); it != vars.end(); ++it)
00285     {
00286         std::set<std::string> cvars;
00287         for (std::set<std::string>::const_iterator itv = (*it).begin();
00288              itv != (*it).end(); ++itv)
00289         {
00290             if ((*itv)[0] == 'E' || (*itv)[0] == 'R' || (*itv)[0] == 'V') {
00291                 cvars.insert(*itv);
00292             }
00293         }
00294         photometricVars.push_back(cvars);
00295         optCount+=cvars.size();
00296     }
00297     //if no variables to optimize return
00298     if(optCount==0)
00299     {
00300         return;
00301     };
00302 
00303     int nMaxIter = 250;
00304     OptimData data(pano, photometricVars, correspondences, 5/255.0, false, nMaxIter, progress);
00305 
00306     int ret;
00307     //double opts[LM_OPTS_SZ];
00308     double info[LM_INFO_SZ];
00309 
00310     // parameters
00311     int m=data.m_vars.size();
00312     vigra::ArrayVector<double> p(m, 0.0);
00313 
00314     // vector for errors
00315     int n=2*3*correspondences.size()+pano.getNrOfImages();
00316     vigra::ArrayVector<double> x(n, 0.0);
00317 
00318     data.ToX(p.begin());
00319 #ifdef DEBUG
00320     printf("Parameters before optimisation: ");
00321     for(int i=0; i<m; ++i)
00322         printf("%.7g ", p[i]);
00323     printf("\n");
00324 #endif
00325 
00326     // covariance matrix at solution
00327     vigra::DImage cov(m,m);
00328     // TODO: setup optimisation options with some good defaults.
00329     double optimOpts[5];
00330     
00331     optimOpts[0] = 1E-03;  // init mu
00332     // stop thresholds
00333     optimOpts[1] = 1e-5;   // ||J^T e||_inf
00334     optimOpts[2] = 1e-5;   // ||Dp||_2
00335     optimOpts[3] = 1e-1;   // ||e||_2
00336     // difference mode
00337     optimOpts[4] = LM_DIFF_DELTA;
00338     
00339     ret=dlevmar_dif(&photometricError, &photometricVis, &(p[0]), &(x[0]), m, n, nMaxIter, optimOpts, info, NULL, &(cov(0,0)), &data);  // no jacobian
00340     // copy to source images (data.m_imgs)
00341     data.FromX(p.begin());
00342     // calculate error at solution
00343     data.huberSigma = 0;
00344     photometricError(&(p[0]), &(x[0]), m, n, &data);
00345     error = 0;
00346     for (int i=0; i<n; i++) {
00347         error += x[i]*x[i];
00348     }
00349     error = sqrt(error/n);
00350 
00351 #ifdef DEBUG
00352     printf("Levenberg-Marquardt returned %d in %g iter, reason %g\nSolution: ", ret, info[5], info[6]);
00353     for(int i=0; i<m; ++i)
00354         printf("%.7g ", p[i]);
00355     printf("\n\nMinimization info:\n");
00356     for(int i=0; i<LM_INFO_SZ; ++i)
00357         printf("%g ", info[i]);
00358     printf("\n");
00359 #endif
00360 
00361     // copy settings to panorama
00362     for (unsigned i=0; i<pano.getNrOfImages(); i++) {
00363         pano.setSrcImage(i, data.m_imgs[i]);
00364     }
00365 }
00366 
00367 void SmartPhotometricOptimizer::smartOptimizePhotometric(PanoramaData & pano, PhotometricOptimizeMode mode,
00368                                                           const std::vector<vigra_ext::PointPairRGB> & correspondences,
00369                                                           AppBase::ProgressReporter & progress,
00370                                                           double & error)
00371 {
00372     std::vector<SrcPanoImage> ret;
00373     PanoramaOptions opts = pano.getOptions();
00374     if (mode == OPT_PHOTOMETRIC_LDR || mode == OPT_PHOTOMETRIC_LDR_WB) {
00375         // optimize exposure
00376         int vars = OPT_EXP;
00377         optimizePhotometric(pano, 
00378                             createOptVars(pano, vars, opts.colorReferenceImage),
00379                             correspondences, progress, error);
00380 
00381         // optimize vignetting & exposure
00382         vars = OPT_EXP | OPT_VIG;
00383         optimizePhotometric(pano, 
00384                             createOptVars(pano, vars, opts.colorReferenceImage),
00385                             correspondences, progress, error);
00386 
00387         if (mode == OPT_PHOTOMETRIC_LDR_WB) {
00388             // optimize vignetting & exposure & wb & response
00389             vars = OPT_EXP | OPT_VIG | OPT_RESP | OPT_WB;
00390         } else {
00391             vars = OPT_EXP | OPT_VIG | OPT_RESP;
00392         }
00393         // optimize vignetting & exposure & wb & response
00394         optimizePhotometric(pano, 
00395                             createOptVars(pano, vars, opts.colorReferenceImage),
00396                             correspondences, progress, error);
00397     } else if (mode == OPT_PHOTOMETRIC_HDR || mode == OPT_PHOTOMETRIC_HDR_WB) {
00398         // optimize vignetting
00399         int vars = OPT_VIG;
00400         optimizePhotometric(pano, 
00401                             createOptVars(pano, vars, opts.colorReferenceImage),
00402                             correspondences, progress, error);
00403 
00404         // optimize vignetting, wb and response
00405         if (mode == OPT_PHOTOMETRIC_HDR_WB) {
00406             vars = OPT_VIG | OPT_RESP | OPT_WB;
00407         } else {
00408             vars =  OPT_VIG | OPT_RESP;
00409         }
00410         optimizePhotometric(pano, 
00411                             createOptVars(pano, vars, opts.colorReferenceImage),
00412                             correspondences, progress, error);
00413     } else {
00414         assert(0 && "Unknown photometric optimisation mode");
00415     }
00416     // adjust 
00417 }
00418 
00419 
00420 bool PhotometricOptimizer::runAlgorithm()
00421 {
00422     // is this correct? how much progress requierd?
00423     AppBase::ProgressReporter* progRep = 
00424         AppBase::ProgressReporterAdaptor::newProgressReporter(getProgressDisplay(), 0.0);    
00425     
00426     optimizePhotometric(o_panorama, 
00427                         o_vars, o_correspondences,
00428                         *progRep, o_resultError);
00429     
00430     delete progRep;
00431     
00432     // optimizePhotometric does not tell us if it's cancelled
00433     if(hasProgressDisplay())
00434     {
00435         if(getProgressDisplay()->wasCancelled())
00436             cancelAlgorithm();
00437     }
00438     
00439     return wasCancelled(); // let's hope so.
00440 }
00441 
00442 bool SmartPhotometricOptimizer::runAlgorithm()
00443 {
00444     AppBase::ProgressReporter* progRep;
00445     
00446     if(hasProgressDisplay()) {
00447         // is this correct? how much progress requierd?
00448         progRep = new AppBase::ProgressReporterAdaptor(*getProgressDisplay(), 0.0); 
00449     } else {
00450         progRep = new AppBase::DummyProgressReporter();
00451     }
00452     
00453     smartOptimizePhotometric(o_panorama,
00454                              o_optMode,
00455                              o_correspondences,
00456                              *progRep, o_resultError);
00457     
00458     delete progRep;
00459     
00460     // smartOptimizePhotometric does not tell us if it's cancelled
00461     if(hasProgressDisplay())
00462     {
00463         if(getProgressDisplay()->wasCancelled())
00464             cancelAlgorithm();
00465     }
00466     
00467     return !wasCancelled(); // let's hope so.
00468 }
00469 
00470 } //namespace

Generated on 28 Nov 2014 for Hugintrunk by  doxygen 1.4.7