/******************************************************************** ** Image Component Library (ICL) ** ** ** ** Copyright (C) 2006-2012 CITEC, University of Bielefeld ** ** Neuroinformatics Group ** ** Website: www.iclcv.org and ** ** http://opensource.cit-ec.de/projects/icl ** ** ** ** File : ICLGeom/examples/kinect-rgbd-calib.cpp ** ** Module : ICLGeom ** ** Authors: Andre Ueckermann, Christof Elbrechter ** ** ** ** ** ** Commercial License ** ** ICL can be used commercially, please refer to our website ** ** www.iclcv.org for more details. ** ** ** ** GNU General Public License Usage ** ** Alternatively, this file may be used under the terms of the ** ** GNU General Public License version 3.0 as published by the ** ** Free Software Foundation and appearing in the file LICENSE.GPL ** ** included in the packaging of this file. Please review the ** ** following information to ensure the GNU General Public License ** ** version 3.0 requirements will be met: ** ** http://www.gnu.org/copyleft/gpl.html. ** ** ** ** The development of this software was supported by the ** ** Excellence Cluster EXC 277 Cognitive Interaction Technology. ** ** The Excellence Cluster EXC 277 is a grant of the Deutsche ** ** Forschungsgemeinschaft (DFG) in the context of the German ** ** Excellence Initiative. ** ** ** *********************************************************************/ #include <ICLQuick/Common.h> #include <ICLUtils/ConfigFile.h> #include <ICLMarkers/FiducialDetector.h> #include <ICLGeom/PointNormalEstimation.h> #include <ICLUtils/Homography2D.h> #include <ICLCore/Random.h> #include <ICLUtils/ConsoleProgress.h> #include <ICLUtils/SimplexOptimizer.h> PointNormalEstimation *pEst; ImageUndistortion udistRGB; ImageUndistortion udistIR; GUI gui("hsplit"); GenericGrabber grabDepth, grabColor; SmartPtr<FiducialDetector> fid,fid2; std::vector<std::vector<float> > correspondences; std::vector<std::string> names; //FixedMatrix<float,3,3> H; Homography2D H; Img32f matchImage(Size(320,240), formatRGB); GUI fidDetectorPropertyGUI("hsplit"); float compute_homograpy_error(const Point32f *as, const Point32f *bs, int N, const Homography2D &h, int &outlierCounter, int outlierThreshold, std::vector<int> &inliners){ float err = 0; for(int i=0;i<N;++i){ float e = as[i].distanceTo(h.apply(bs[i])); if(e < outlierThreshold){ err += e; inliners.push_back(i); }else{ ++outlierCounter; } } if(outlierCounter == N) return 1E20; return err / (N-outlierCounter); } Homography2D ransacBasedHomography(const Point32f *as, const Point32f *bs, int N, std::vector<int> *inliners=0, int numSteps=100000, float outlierThreshold=2){ if(N < 5) throw ICLException("unabled to apply ransac based homography " "estimation on less than 5 points"); Homography2D hBest; float errBest = 0; int numOutliersBest = 0; std::vector<int> bestInliners; std::cout << "starting ransac ..." << std::endl; URandI ridx(N-1); progress_init("running ransac optimization"); for(int i=0;i<numSteps;++i){ if(!(i%100)){ progress(i,numSteps-1); } int a = ridx; int b = ridx; while(b == a) b = ridx; int c = ridx; while(c==a || c==b) c = ridx; int d = ridx; while(d==a || d==b || d==c) d = ridx; const Point32f pa[4] = { as[a], as[b], as[c], as[d] }; const Point32f pb[4] = { bs[a], bs[b], bs[c], bs[d] }; Homography2D h(pa,pb,4); int numOutliers = 0; std::vector<int> inlinerIndices; float err = compute_homograpy_error(as,bs,N,h, numOutliers, outlierThreshold, inlinerIndices); if(!i || (numOutliers < numOutliersBest) || (numOutliers==numOutliersBest && err<errBest)) { hBest = h; errBest = err; numOutliersBest = numOutliers; bestInliners = inlinerIndices; } } progress_finish(); std::cout << "\n#outliers:" << numOutliersBest << "\n error:" << errBest << "\n"; if(inliners) *inliners = bestInliners; return hBest; } struct Err{ const std::vector<int> &inliners; const Point32f *as; const Point32f *bs; float f(const std::vector<float> ¶ms) const{ Homography2D h; std::copy(params.begin(), params.end(), h.begin()); h(2,2) = 1; // SHOW(h); float err = 0; for(unsigned int i=0;i<inliners.size();++i){ err += as[inliners[i]].distanceTo(h.apply(bs[inliners[i]])); } return err / inliners.size(); } }; //void step_callback(const SimplexOptimizer<float,std::vector<float> >::Result &res){ // std::cout << " simplex error: " << res.fx << std::endl; //} Homography2D simplexBasedHomography(const Point32f *as, const Point32f *bs, int N){ typedef SimplexOptimizer<float,std::vector<float> > Simplex; std::vector<int> inliners; Homography2D h = ransacBasedHomography(as,bs,N,&inliners,10000); Err err = {inliners, as, bs}; Simplex simplex(function(err,&Err::f), 8, 1E6, 1.E-5, 1.E-15); std::cout << "running simplex optimization " << std::endl; Simplex::Result res = simplex.optimize(std::vector<float>(h.begin(), h.begin()+8)); std::cout << "simplex error: " << res.fx << std::endl; std::copy(res.x.begin(), res.x.end(),h.begin()); return h; } void init(){ Size size = pa("-size"); pEst=new PointNormalEstimation(size); matchImage.setSize(size); grabDepth.init("kinectd","kinectd=0"); grabColor.init("kinectc","kinectc=0"); grabDepth.useDesired(depth32f, size, formatMatrix); grabColor.useDesired(depth32f, size, formatMatrix); fid = new FiducialDetector(pa("-m").as<std::string>(), pa("-m",1).as<std::string>(), ParamList("size",(*pa("-m",2)) ) ); fid2 = new FiducialDetector(pa("-m").as<std::string>(), pa("-m",1).as<std::string>(), ParamList("size",(*pa("-m",2)) ) ); fid->setConfigurableID("fid"); fid2->setConfigurableID("fid2"); gui << (GUI("vsplit") << "draw[@handle=color@minsize=16x12@label=color image]" << "draw[@handle=ir@minsize=16x12@label=IR image]" ) << (GUI("vsplit") << "draw[@handle=match@minsize=16x12@label=result visualization]" << "draw[@handle=depth@minsize=16x12@label=depth image]" ) << ( GUI("vbox[@minsize=12x2]") << "togglebutton(pause, run)[@out=paused]" << ("combo(R/G mapped_RGB/depth,|mapped_RGB - IR|,!R/B mapped_RGB/IR)" "[@handle=resultVis@label=result visualization method@maxsize=99x3]") << "combo(least squares,ransac, ransac+simplex)[@handle=optMethod@label=optimization method]" << "checkbox(use corners,unchecked)[@out=useCorners]" << "button(add points)[@handle=addPoints]" << "button(calculate homography)[@handle=calcHomo]" << "button(save homography)[@handle=saveHomo]" << "button(clear points and reset)[@handle=clearPoints]" // << (GUI("hbox") << "button(more ...)[@handle=more]" // << "camcfg" // ) ) << "!show"; fidDetectorPropertyGUI << (GUI("vbox[@maxsize=16x100]") << ("combo(" + fid->getIntermediateImageNames() + ")" "[@maxsize=100x2@handle=vis@label=color image]") << "prop(fid)") << (GUI("vbox[@maxsize=16x100]") << ("combo(" + fid2->getIntermediateImageNames() + ")" "[@maxsize=100x2@handle=vis2@label=infrared image]") << "prop(fid2)") << "!create"; gui["more"].registerCallback(function(&fidDetectorPropertyGUI,&GUI::switchVisibility)); try{ fid->setPropertyValue("thresh.algorithm","tiled linear"); fid2->setPropertyValue("thresh.algorithm","region mean"); fid2->setPropertyValue("matching algorithm","gray ncc"); fid2->setPropertyValue("thresh.global threshold","-0.7"); fid2->setPropertyValue("thresh.mask size","19"); fid2->setPropertyValue("quads.minimum region size","360"); }catch(ICLException &e){ WARNING_LOG("exception caught while setting initial parameters: " << e.what()); } gui_DrawHandle(depth); depth->setRangeMode(ICLWidget::rmAuto); gui_DrawHandle(match); match->setRangeMode(ICLWidget::rmAuto); if(pa("-rgb-udist")){ string fn1 = pa("-rgb-udist"); udistRGB=ImageUndistortion(fn1); grabColor.enableUndistortion(udistRGB); std::cout<<"RGB-UNDISTORTION: --- "<<fn1<<" --- "<<std::endl<<udistRGB<<std::endl; } if(pa("-ir-udist")){ string fn2 = pa("-ir-udist"); udistIR=ImageUndistortion(fn2); grabDepth.enableUndistortion(udistIR); std::cout<<"IR-UNDISTORTION: --- "<<fn2<<" --- "<<std::endl<<udistIR<<std::endl; } H.at(0,0)=1; H.at(1,1)=1; H.at(2,2)=1; } void visualizeMatches(DrawHandle &draw, const std::vector<Fiducial> &fids, FiducialDetector *fd, const std::string &imageName, bool showCorrespondances){ draw = fd->getIntermediateImage(imageName); draw->lock(); draw->reset(); draw->linewidth(1); draw->symsize(20); for(unsigned int i=0;i<fids.size();++i){ draw->color(0,100,255,200); draw->linestrip(fids[i].getCorners2D()); draw->color(255,0,0,255); draw->sym(fids[i].getCenter2D(),'x'); } draw->color(0,255,0,255); draw->fill(0,0,0,0); if(showCorrespondances){ for(unsigned int i=0;i<correspondences.size(); ++i){ const std::vector<float> &v = correspondences[i]; draw->sym(Point32f(v[0],v[1]), 'o'); } } draw->unlock(); } void run(){ gui_ButtonHandle(addPoints); gui_ButtonHandle(clearPoints); gui_ButtonHandle(calcHomo); gui_ButtonHandle(saveHomo); Size size = pa("-size"); static Img32f C,IR,D; Img32f IRmed; grabDepth.grab(bpp(D)); grabDepth.grab(); grabColor.disableUndistortion(); grabColor.setProperty("format","Color Image (24Bit RGB)"); if(pa("-rgb-udist")){ grabColor.enableUndistortion(udistRGB); } grabColor.grab(); grabColor.grab(bpp(C)); grabColor.disableUndistortion(); grabColor.setProperty("format","IR Image (8Bit)"); if(pa("-do-not-shift-the-IR-image")){ grabColor.setProperty("shift-IR-image", "accurate"); }else{ grabColor.setProperty("shift-IR-image", "off"); } if(pa("-ir-udist")){ grabColor.enableUndistortion(udistIR); } grabColor.grab(); grabColor.grab(bpp(IR)); if(size==Size::QVGA){ pEst->setMedianFilterSize(3); pEst->setDepthImage(IR); pEst->medianFilter(); IRmed=pEst->getFilteredImage(); }else if(size==Size::VGA){ pEst->setMedianFilterSize(5); pEst->setDepthImage(IR); pEst->medianFilter(); IRmed=pEst->getFilteredImage(); }else{ IRmed=IR; } gui["ir"] = IRmed; gui["depth"] = D; gui["color"] = C; const std::vector<Fiducial> &fids = fid->detect(&C); const std::vector<Fiducial> &fids2 = fid2->detect(&IRmed); static DrawHandle color = gui["color"]; static DrawHandle ir = gui["ir"]; visualizeMatches(color,fids, fid.get(), fidDetectorPropertyGUI["vis"], true); visualizeMatches(ir,fids2, fid2.get(), fidDetectorPropertyGUI["vis2"], false); const Rect r = C.getImageRect(); Channel32f c = C[0]; switch(gui["resultVis"].as<int>()){ case 0:{ matchImage.setChannels(2); std::copy(D.begin(0),D.end(0),matchImage.begin(1)); Channel32f miC0 = matchImage[0]; for(int y=0; y<size.height; y++){ for(int x=0; x<size.width; x++){ Point p = H.apply_int(Point32f(x,y)); miC0(x,y) = r.contains(p.x,p.y) ? c(p.x,p.y) : 0.0f; } } break; } case 1:{ matchImage.setChannels(1); Channel32f miC0 = matchImage[0]; Channel32f irC0 = IR[0]; for(int y=0; y<size.height; y++){ for(int x=0; x<size.width; x++){ Point p = H.apply_int(Point32f(x,y)); miC0(x,y) = r.contains(p.x,p.y) ? ::abs(irC0(x,y) - c(p.y,p.y)) : 0.0f; } } break; } case 2:{ matchImage.setChannels(2); std::copy(IR.begin(0),IR.end(0),matchImage.begin(1)); Channel32f miC0 = matchImage[0]; for(int y=0; y<size.height; y++){ for(int x=0; x<size.width; x++){ Point p = H.apply_int(Point32f(x,y)); miC0(x,y) = r.contains(p.x,p.y) ? c(p.x,p.y) : 0.0f; } } break; } } matchImage.normalizeAllChannels(Range32f(0,255)); gui["match"] = matchImage; gui["depth"].update(); gui["color"].update(); gui["ir"].update(); gui["match"].update(); while(gui["paused"]){} if(clearPoints.wasTriggered()){ correspondences.clear(); names.clear(); (FixedMatrix<float,3,3>&)H=FixedMatrix<float,3,3>::id(); } if(addPoints.wasTriggered()){ bool useCorners = gui["useCorners"]; std::cout<<"COLOR POINTS: "<<std::endl; for(unsigned int i=0; i<fids.size(); i++){ std::cout<<fids[i].getName()<<", "<<fids[i].getCenter2D().x<<"x"<<fids[i].getCenter2D().y<<std::endl; } std::cout<<"IR POINTS: "<<std::endl; for(unsigned int i=0; i<fids2.size(); i++){ std::cout<<fids2[i].getName()<<", "<<fids2[i].getCenter2D().x<<"x"<<fids2[i].getCenter2D().y<<std::endl; } for(unsigned int a=0;a<fids.size(); a++){ const Fiducial &fa = fids[a]; for(unsigned int b=0; b<fids2.size(); b++){ const Fiducial &fb = fids2[b]; if(fa.getName()==fb.getName()){ const float d[4] = {fa.getCenter2D().x, fa.getCenter2D().y, fb.getCenter2D().x, fb.getCenter2D().y }; correspondences.push_back(std::vector<float>(d,d+4)); names.push_back(fids[a].getName()); if(useCorners){ const std::vector<Fiducial::KeyPoint> &ka = fa.getKeyPoints2D(); const std::vector<Fiducial::KeyPoint> &kb = fb.getKeyPoints2D(); for(unsigned int i=0;i<ka.size();++i){ const float d[4] = { ka[i].imagePos.x, ka[i].imagePos.y, kb[i].imagePos.x, kb[i].imagePos.y }; correspondences.push_back(std::vector<float>(d,d+4)); names.push_back(fids[a].getName() + " key-point: " +str(i)); } } break; } } } std::cout<<correspondences.size()<<"CORRESPONDENCES: "<<std::endl; for(unsigned int c=0;c<correspondences.size(); c++){ std::cout<<"Color: "<<correspondences[c][0]<<"x"<<correspondences[c][1]<<" , IR: "<<correspondences[c][2]<<"x"<<correspondences[c][3]<<std::endl; } } if(calcHomo.wasTriggered()){ const int Ncorr = correspondences.size(); if(Ncorr>3){ std::vector<Point32f> A(Ncorr),B(Ncorr); for(int i=0;i<Ncorr; ++i){ A[i].x=correspondences[i][0]; A[i].y=correspondences[i][1]; B[i].x=correspondences[i][2]; B[i].y=correspondences[i][3]; std::cout<<A[i]<<" , "<<B[i]<<std::endl; } switch(gui["optMethod"].as<int>()){ case 0: H = Homography2D(A.data(),B.data(),Ncorr); break; case 1: H = ransacBasedHomography(A.data(), B.data(), Ncorr); break; case 2: H = simplexBasedHomography(A.data(), B.data(), Ncorr); break; } std::cout << H << std::endl; float errorsum=0; for(unsigned int i=0; i<correspondences.size(); i++){ float xi = correspondences[i][0]; float yi = correspondences[i][1]; float x = correspondences[i][2]; float y = correspondences[i][3]; float az = H(0,2)*x + H(1,2) * y + H(2,2); int ax = round(( H(0,0)*x + H(1,0) * y + H(2,0) ) / az); int ay = round(( H(0,1)*x + H(1,1) * y + H(2,1) ) / az); float dist=sqrt(sqr(xi-ax)+sqr(yi-ay)); errorsum+=dist; std::cout<<"Point "<<i+1<<"("<<names[i]<<") Error (Distance): "<<dist<<std::endl;// } std::cout<<"Full Error: "<<errorsum<<std::endl; }else{ std::cout<<"Too few points for calculation, please add points."<<std::endl; } } if(saveHomo.wasTriggered()){ try{ ConfigFile f; f["config.homogeneity"] = (FixedMatrix<float,3,3>&)H; // !homography ?!?! f.save(*pa("-o")); std::cout << "saved homography to file " << *pa("-o") << std::endl; }catch(ICLException &ex){ ERROR_LOG(ex.what()); } } } int main(int n, char **ppc){ return ICLApp(n,ppc, "-size|-s(Size=VGA) " "-output|-o(output-xml-file-name=homogeneity.xml) " "-rgb-udist(fn1) " "-ir-udist(fn2) " "-marker-type|-m(type=bch,whichToLoad=[0-1000],size=50x50) " "-do-not-shift-the-IR-image", init,run).exec(); }