CameraCalibrator.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "ptam/OpenGL.h"
00003 #include <gvars3/instances.h>
00004 #include "ptam/CameraCalibrator.h"
00005 #include <ptam/Params.h>
00006 #include <TooN/SVD.h>
00007 #include <fstream>
00008 #include <stdlib.h>
00009 
00010 #include <cvd/image.h>
00011 #include <cvd/byte.h>
00012 #include <cvd/rgb.h>
00013 #include <cvd/utility.h>
00014 
00015 #include <ros/ros.h>
00016 #include <ros/package.h>
00017 
00018 using namespace CVD;
00019 using namespace std;
00020 using namespace GVars3;
00021 
00022 //Weiss{
00023 Vector<NUMTRACKERCAMPARAMETERS> camparams;
00024 //}
00025 
00026 int main(int argc, char** argv)
00027 {
00028 
00029   ros::init(argc, argv, "cameracalibrator");
00030   ROS_INFO("starting CameraCalibrator with node name %s", ros::this_node::getName().c_str());
00031 
00032   cout << "  Welcome to CameraCalibrator " << endl;
00033   cout << "  -------------------------------------- " << endl;
00034   cout << "  Parallel tracking and mapping for Small AR workspaces" << endl;
00035   cout << "  Copyright (C) Isis Innovation Limited 2008 " << endl;
00036 
00037   GUI.StartParserThread();
00038   atexit(GUI.StopParserThread); // Clean up readline when program quits
00039   GV3::get<Vector<NUMTRACKERCAMPARAMETERS> >("Camera.Parameters", ATANCamera::mvDefaultParams, SILENT);
00040 
00041   try
00042   {
00043     std::cout<<"Gui is "<<(PtamParameters::fixparams().gui ? "on" : "off")<<std::endl; //make the singleton instantiate
00044     CameraCalibrator c;
00045     c.Run();
00046   }
00047   catch (CVD::Exceptions::All& e)
00048   {
00049     cout << endl;
00050     cout << "!! Failed to run CameraCalibrator; got exception. " << endl;
00051     cout << "   Exception was: " << endl;
00052     cout << e.what << endl;
00053   }
00054 }
00055 
00056 void CameraCalibrator::imageCallback(const sensor_msgs::ImageConstPtr & img)
00057 {
00058 
00059   ROS_ASSERT(img->encoding == sensor_msgs::image_encodings::MONO8 && img->step == img->width);
00060 
00061   CVD::ImageRef size(img->width, img->height);
00062   mCurrentImage.resize(size);
00063 
00064   CVD::BasicImage<CVD::byte> img_tmp((CVD::byte *)&img->data[0], size);
00065   CVD::copy(img_tmp, mCurrentImage);
00066   mNewImage = true;
00067 }
00068 
00069 CameraCalibrator::CameraCalibrator() :
00070       mCamera("Camera"), mbDone(false), mCurrentImage(CVD::ImageRef(752, 480)), mDoOptimize(false), mNewImage(false)
00071 {
00072   ros::NodeHandle nh;
00073   image_transport::ImageTransport it(nh);
00074   mImageSub = it.subscribe("image", 1, &CameraCalibrator::imageCallback, this);
00075 }
00076 
00077 CameraCalibrator::~CameraCalibrator()
00078 {
00079   delete mGLWindow;
00080 }
00081 
00082 void CameraCalibrator::init()
00083 {
00084   mGLWindow = new GLWindow2(mCurrentImage.size(), "Camera Calibrator");
00085 
00086   GUI.RegisterCommand("CameraCalibrator.GrabNextFrame", GUICommandCallBack, this);
00087   GUI.RegisterCommand("CameraCalibrator.Reset", GUICommandCallBack, this);
00088   GUI.RegisterCommand("CameraCalibrator.ShowNext", GUICommandCallBack, this);
00089   GUI.RegisterCommand("CameraCalibrator.SaveCalib", GUICommandCallBack, this);
00090   GUI.RegisterCommand("quit", GUICommandCallBack, this);
00091   GUI.RegisterCommand("exit", GUICommandCallBack, this);
00092 
00093   GV3::Register(mgvnOptimizing, "CameraCalibrator.Optimize", 0, SILENT);
00094   GV3::Register(mgvnShowImage, "CameraCalibrator.Show", 0, SILENT);
00095   GV3::Register(mgvnDisableDistortion, "CameraCalibrator.NoDistortion", 0, SILENT);
00096 
00097   GUI.ParseLine("GLWindow.AddMenu CalibMenu");
00098   GUI.ParseLine("CalibMenu.AddMenuButton Live GrabFrame CameraCalibrator.GrabNextFrame");
00099   GUI.ParseLine("CalibMenu.AddMenuButton Live Reset CameraCalibrator.Reset");
00100   GUI.ParseLine("CalibMenu.AddMenuButton Live Optimize \"CameraCalibrator.Optimize=1\"");
00101   GUI.ParseLine("CalibMenu.AddMenuToggle Live NoDist CameraCalibrator.NoDistortion");
00102   GUI.ParseLine("CalibMenu.AddMenuSlider Opti \"Show Img\" CameraCalibrator.Show 0 10");
00103   GUI.ParseLine("CalibMenu.AddMenuButton Opti \"Show Next\" CameraCalibrator.ShowNext");
00104   GUI.ParseLine("CalibMenu.AddMenuButton Opti \"Grab More\" CameraCalibrator.Optimize=0 ");
00105   GUI.ParseLine("CalibMenu.AddMenuButton Opti Reset CameraCalibrator.Reset");
00106   GUI.ParseLine("CalibMenu.AddMenuToggle Opti NoDist CameraCalibrator.NoDistortion");
00107   GUI.ParseLine("CalibMenu.AddMenuButton Opti Save CameraCalibrator.SaveCalib");
00108   Reset();
00109 }
00110 
00111 void CameraCalibrator::Run()
00112 {
00113   bool initialized = false;
00114   ros::Rate r(100);
00115   while (!mbDone && ros::ok())
00116   {
00117 
00118     ros::spinOnce();
00119     r.sleep();
00120 
00121     if (!initialized)
00122     {
00123       if (mNewImage)
00124       {
00125         init();
00126         initialized = true;
00127       }
00128       continue;
00129     }
00130 
00131     // Set up openGL
00132     mGLWindow->SetupViewport();
00133     mGLWindow->SetupVideoOrtho();
00134     mGLWindow->SetupVideoRasterPosAndZoom();
00135 
00136     if (mvCalibImgs.size() < 1)
00137       *mgvnOptimizing = false;
00138 
00139     if (!*mgvnOptimizing)
00140     {
00141       GUI.ParseLine("CalibMenu.ShowMenu Live");
00142       glDrawPixels(mCurrentImage);
00143       mDoOptimize = true; // set this so that optimization begins when "optimize" is pressed)
00144 
00145       if (mNewImage)
00146       {
00147         mNewImage = false;
00148         CalibImage c;
00149         if (c.MakeFromImage(mCurrentImage))
00150         {
00151           if (mbGrabNextFrame)
00152           {
00153             mvCalibImgs.push_back(c);
00154             mvCalibImgs.back().GuessInitialPose(mCamera);
00155             mvCalibImgs.back().Draw3DGrid(mCamera, false);
00156             mbGrabNextFrame = false;
00157           };
00158         }
00159       }
00160     }
00161     else
00162     {
00163       if(mDoOptimize)
00164         OptimizeOneStep();
00165       GUI.ParseLine("CalibMenu.ShowMenu Opti");
00166       int nToShow = *mgvnShowImage - 1;
00167       if (nToShow < 0)
00168         nToShow = 0;
00169       if (nToShow >= (int)mvCalibImgs.size())
00170         nToShow = mvCalibImgs.size() - 1;
00171       *mgvnShowImage = nToShow + 1;
00172 
00173       glDrawPixels(mvCalibImgs[nToShow].mim);
00174       mvCalibImgs[nToShow].Draw3DGrid(mCamera, true);
00175     }
00176 
00177     ostringstream ost;
00178     ost << "Camera Calibration: Grabbed " << mvCalibImgs.size() << " images." << endl;
00179     if (!*mgvnOptimizing)
00180     {
00181       ost << "Take snapshots of the calib grid with the \"GrabFrame\" button," << endl;
00182       ost << "and then press \"Optimize\"." << endl;
00183       ost << "Take enough shots (4+) at different angles to get points " << endl;
00184       ost << "into all parts of the image (corners too.) The whole grid " << endl;
00185       ost << "doesn't need to be visible so feel free to zoom in." << endl;
00186     }
00187     else
00188     {
00189       ost << "Current RMS pixel error is " << mdMeanPixelError << endl;
00190       ost << "Current camera params are  " << mCamera.mgvvCameraParams << endl;
00191       ost << "(That would be a pixel aspect ratio of " << mCamera.PixelAspectRatio() << ")" << endl;
00192       ost << "Check fit by looking through the grabbed images." << endl;
00193       ost << "RMS should go below 0.5, typically below 0.3 for a wide lens." << endl;
00194       ost << "Press \"save\" to save calibration to camera.cfg file and exit." << endl;
00195     }
00196 
00197     mGLWindow->DrawCaption(ost.str());
00198     mGLWindow->DrawMenus();
00199     mGLWindow->HandlePendingEvents();
00200     mGLWindow->swap_buffers();
00201   }
00202 }
00203 
00204 void CameraCalibrator::Reset()
00205 {
00206   mCamera.mgvvCameraParams = ATANCamera::mvDefaultParams;
00207   if (*mgvnDisableDistortion)
00208     mCamera.DisableRadialDistortion();
00209 
00210   mCamera.SetImageSize(mCurrentImage.size());
00211   mbGrabNextFrame = false;
00212   *mgvnOptimizing = false;
00213   mvCalibImgs.clear();
00214 }
00215 
00216 void CameraCalibrator::GUICommandCallBack(void* ptr, string sCommand, string sParams)
00217 {
00218   ((CameraCalibrator*)ptr)->GUICommandHandler(sCommand, sParams);
00219 }
00220 
00221 void CameraCalibrator::GUICommandHandler(string sCommand, string sParams) // Called by the callback func..
00222 {
00223   if (sCommand == "CameraCalibrator.Reset")
00224   {
00225     Reset();
00226     return;
00227   };
00228   if (sCommand == "CameraCalibrator.GrabNextFrame")
00229   {
00230     mbGrabNextFrame = true;
00231     return;
00232   }
00233   if (sCommand == "CameraCalibrator.ShowNext")
00234   {
00235     int nToShow = (*mgvnShowImage - 1 + 1) % mvCalibImgs.size();
00236     *mgvnShowImage = nToShow + 1;
00237     return;
00238   }
00239   if (sCommand == "CameraCalibrator.SaveCalib")
00240   {
00241     cout << "  Camera calib is " << mCamera.mgvvCameraParams << endl;
00242     cout << "  Saving camera calib to camera.cfg..." << endl;
00243     ofstream ofs("camera.cfg");
00244     if (ofs.good())
00245     {
00246       ofs << "Camera.Parameters=[ " << mCamera.mgvvCameraParams << " ]";
00247       ofs.close();
00248       cout << "  .. saved." << endl;
00249     }
00250     else
00251     {
00252       cout << "! Could not open camera.cfg for writing." << endl;
00253       GV2.PrintVar("Camera.Parameters", cout);
00254       cout << "  Copy-paste above line to settings.cfg or camera.cfg! " << endl;
00255     }
00256     mbDone = true;
00257   }
00258   if (sCommand == "exit" || sCommand == "quit")
00259   {
00260     mbDone = true;
00261   }
00262 }
00263 
00264 void CameraCalibrator::OptimizeOneStep()
00265 {
00266   int nViews = mvCalibImgs.size();
00267   int nDim = 6 * nViews + NUMTRACKERCAMPARAMETERS;
00268   int nCamParamBase = nDim - NUMTRACKERCAMPARAMETERS;
00269 
00270   Matrix<> mJTJ(nDim, nDim);
00271   Vector<> vJTe(nDim);
00272   mJTJ = Identity; // Weak stabilizing prior
00273   vJTe = Zeros;
00274 
00275   if(*mgvnDisableDistortion) mCamera.DisableRadialDistortion();
00276 
00277 
00278   double dSumSquaredError = 0.0;
00279   int nTotalMeas = 0;
00280 
00281   for(int n=0; n<nViews; n++)
00282   {
00283     int nMotionBase = n*6;
00284     vector<CalibImage::ErrorAndJacobians> vEAJ = mvCalibImgs[n].Project(mCamera);
00285 
00286     for(unsigned int i=0; i<vEAJ.size(); i++)
00287     {
00288       CalibImage::ErrorAndJacobians &EAJ = vEAJ[i];
00289       // All the below should be +=, but the MSVC compiler doesn't seem to understand that. :(
00290       mJTJ.slice(nMotionBase, nMotionBase, 6, 6) = 
00291           mJTJ.slice(nMotionBase, nMotionBase, 6, 6) + EAJ.m26PoseJac.T() * EAJ.m26PoseJac;
00292       mJTJ.slice(nCamParamBase, nCamParamBase, NUMTRACKERCAMPARAMETERS, NUMTRACKERCAMPARAMETERS) = 
00293           mJTJ.slice(nCamParamBase, nCamParamBase, NUMTRACKERCAMPARAMETERS, NUMTRACKERCAMPARAMETERS) + EAJ.m2NCameraJac.T() * EAJ.m2NCameraJac;
00294       mJTJ.slice(nMotionBase, nCamParamBase, 6, NUMTRACKERCAMPARAMETERS) =
00295           mJTJ.slice(nMotionBase, nCamParamBase, 6, NUMTRACKERCAMPARAMETERS) + EAJ.m26PoseJac.T() * EAJ.m2NCameraJac;
00296       mJTJ.T().slice(nMotionBase, nCamParamBase, 6, NUMTRACKERCAMPARAMETERS) = 
00297           mJTJ.T().slice(nMotionBase, nCamParamBase, 6, NUMTRACKERCAMPARAMETERS) + EAJ.m26PoseJac.T() * EAJ.m2NCameraJac;
00298       // Above does twice the work it needs to, but who cares..
00299 
00300       vJTe.slice(nMotionBase,6) = 
00301           vJTe.slice(nMotionBase,6) + EAJ.m26PoseJac.T() * EAJ.v2Error;
00302       vJTe.slice(nCamParamBase,NUMTRACKERCAMPARAMETERS) = 
00303           vJTe.slice(nCamParamBase,NUMTRACKERCAMPARAMETERS) + EAJ.m2NCameraJac.T() * EAJ.v2Error;
00304 
00305       dSumSquaredError += EAJ.v2Error * EAJ.v2Error;
00306       ++nTotalMeas;
00307     }
00308   };
00309 
00310   if(nTotalMeas == 0)
00311   {
00312     ROS_WARN_THROTTLE(2, "No new measurements, this can happen for wide angle cameras when \"Camera.Project()\" gets invalid");
00313     return;
00314   }
00315 
00316   double lastPixelError = mdMeanPixelError;
00317   mdMeanPixelError = sqrt(dSumSquaredError / nTotalMeas);
00318 
00319   if(std::abs(lastPixelError - mdMeanPixelError) < 1e-6)
00320     mDoOptimize = false;
00321 
00322   SVD<> svd(mJTJ);
00323   Vector<> vUpdate(nDim);
00324   vUpdate= svd.backsub(vJTe);
00325   vUpdate *= 0.1; // Slow down because highly nonlinear...
00326   for(int n=0; n<nViews; n++)
00327     mvCalibImgs[n].mse3CamFromWorld = SE3<>::exp(vUpdate.slice(n * 6, 6)) * mvCalibImgs[n].mse3CamFromWorld;
00328   mCamera.UpdateParams(vUpdate.slice(nCamParamBase, NUMTRACKERCAMPARAMETERS));
00329 };
00330 
00331 
00332 
00333 
00334 
00335 
00336 
00337 
00338 
00339 
00340 
00341 
00342 
00343 
00344 


ptam
Author(s): Markus Achtelik , Stephan Weiss , Simon Lynen
autogenerated on Sun Oct 5 2014 23:52:33