00001 // -*- c++ -*- 00002 // Copyright 2008 Isis Innovation Limited 00003 00004 #ifndef __CALIB_IMAGE_H 00005 #define __CALIB_IMAGE_H 00006 #include "ATANCamera.h" 00007 #include "CalibCornerPatch.h" 00008 #include <vector> 00009 #include <TooN/se3.h> 00010 #include <ros/ros.h> 00011 #include <ros/package.h> 00012 00013 const int N_NOT_TRIED=-1; 00014 const int N_FAILED=-2; 00015 00016 struct CalibGridCorner 00017 { 00018 struct NeighborState 00019 { 00020 NeighborState() {val = N_NOT_TRIED;} 00021 int val; 00022 }; 00023 00024 CalibCornerPatch::Params Params; 00025 CVD::ImageRef irGridPos; 00026 NeighborState aNeighborStates[4]; 00027 00028 Matrix<2> GetSteps(std::vector<CalibGridCorner> &vgc); 00029 Matrix<2> mInheritedSteps; 00030 00031 void Draw(); 00032 00033 double ExpansionPotential(); 00034 }; 00035 00036 class CalibImage 00037 { 00038 public: 00039 00040 bool MakeFromImage(CVD::Image<CVD::byte> &im); 00041 SE3<> mse3CamFromWorld; 00042 void DrawImageGrid(); 00043 void Draw3DGrid(ATANCamera &Camera, bool bDrawErrors); 00044 void GuessInitialPose(ATANCamera &Camera); 00045 00046 struct ErrorAndJacobians 00047 { 00048 Vector<2> v2Error; 00049 Matrix<2,6> m26PoseJac; 00050 Matrix<2,NUMTRACKERCAMPARAMETERS> m2NCameraJac; 00051 }; 00052 00053 std::vector<ErrorAndJacobians> Project(ATANCamera &Camera); 00054 00055 CVD::Image<CVD::byte> mim; 00056 00057 protected: 00058 std::vector<CVD::ImageRef> mvCorners; 00059 std::vector<CalibGridCorner> mvGridCorners; 00060 00061 00062 bool ExpandByAngle(int nSrc, int nDirn); 00063 int NextToExpand(); 00064 void ExpandByStep(int n); 00065 CVD::ImageRef IR_from_dirn(int nDirn); 00066 00067 }; 00068 00069 00070 00071 00072 #endif 00073