[libcvd-members] libcvd cvd/camera.h progs/calibrate.cxx

[Edward Rosten]

CVSROOT:        /cvsroot/libcvd
Module name:    libcvd
Changes by:     Edward Rosten <edrosten>        08/08/29 03:33:27

Modified files:
        cvd            : camera.h 
        progs          : calibrate.cxx 

Log message:
        Modified calibrate to generalize the camera model.
        
        The camera model is altered at compile time. Currently, it isn't 
templated
        becuse of the use of some globals, and initialization like line 672.
        
        Calibrate assumes that there are 4 non radial distortion parameters: 
the 
        optic axis, the focal length and the aspect ratio (stored as 2 focal 
lengths). 
        All remaining parameters are assumed to be radial distortion.
        
        Tested with Quintic, Cubic and Harris models.

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/libcvd/cvd/camera.h?cvsroot=libcvd&r1=1.19&r2=1.20
http://cvs.savannah.gnu.org/viewcvs/libcvd/progs/calibrate.cxx?cvsroot=libcvd&r1=1.8&r2=1.9

Patches:
Index: cvd/camera.h
===================================================================
RCS file: /cvsroot/libcvd/libcvd/cvd/camera.h,v
retrieving revision 1.19
retrieving revision 1.20
diff -u -b -r1.19 -r1.20
--- cvd/camera.h        29 Aug 2008 02:10:12 -0000      1.19
+++ cvd/camera.h        29 Aug 2008 03:33:26 -0000      1.20
@@ -99,7 +99,7 @@
     /// Fast linear projection for working out what's there
     inline TooN::Vector<2> linearproject(const TooN::Vector<2>& camframe, 
double scale=1); 
        /// Project from Euclidean camera frame to image plane
-    inline TooN::Vector<2> project(const TooN::Vector<2>& camframe); 
+    inline TooN::Vector<2> project(const TooN::Vector<2>& camframe) const; 
        /// Project from image plane to a Euclidean camera
     inline TooN::Vector<2> unproject(const TooN::Vector<2>& imframe); 
     
@@ -126,7 +126,7 @@
 
   private:
     TooN::Vector<num_parameters> my_camera_parameters; // f_u, f_v, u_0, v_0
-    TooN::Vector<2> my_last_camframe;
+    mutable TooN::Vector<2> my_last_camframe;
   };
 
 
@@ -244,19 +244,23 @@
                        ///Load parameters from a stream 
                        ///@param is The stream to use
                        inline void load(std::istream& is);
+
                        /// Save parameters to a stream 
                        ///@param os The stream to use
-                       inline void save(std::ostream& os);
+                       inline void save(std::ostream& os)
+                       {
+                               os << my_camera_parameters;
+                       }
 
 
                        /// Fast linear projection for working out what's there
-                       inline TooN::Vector<2> linearproject(const 
TooN::Vector<2>& camframe, double scale=1)
+                       inline TooN::Vector<2> linearproject(const 
TooN::Vector<2>& camframe, double scale=1) const
                        {
                                return TooN::Vector<2>(scale * 
diagmult(camframe, my_camera_parameters.slice<0,2>()) + 
my_camera_parameters.slice<2,2>());
                        }
 
                        /// Project from Euclidean camera frame to image plane
-                       inline TooN::Vector<2> project(const TooN::Vector<2>& 
camframe)
+                       inline TooN::Vector<2> project(const TooN::Vector<2>& 
camframe) const 
                        {
                                my_last_camframe = camframe;
                                return linearproject(radial_distort(camframe));
@@ -318,19 +322,19 @@
 
                                //Derivatives of x_image:
                                result[0][0] = mod_camframe[0];
-                               result[0][1] = 0;
-                               result[0][2] = 1;
-                               result[0][3] = 0;
-                               result[0][4] = - fu * xc * r2 / 2 * g3;
+                               result[1][0] = 0;
+                               result[2][0] = 1;
+                               result[3][0] = 0;
+                               result[4][0] = - fu * xc * r2 / 2 * g3;
 
 
 
                                //Derivatives of y_image:
-                               result[0][0] = 0;
-                               result[0][1] = mod_camframe[1];
-                               result[0][2] = 0;
-                               result[0][3] = 1;
-                               result[0][4] = - fv * yc * r2 / 2 * g3;
+                               result[0][1] = 0;
+                               result[1][1] = mod_camframe[1];
+                               result[2][1] = 0;
+                               result[3][1] = 1;
+                               result[4][1] = - fv * yc * r2 / 2 * g3;
 
                                return result;
                        }
@@ -357,7 +361,7 @@
 
                  private:
                        TooN::Vector<num_parameters> my_camera_parameters; // 
f_u, f_v, u_0, v_0, alpha
-                       TooN::Vector<2> my_last_camframe;
+                       mutable TooN::Vector<2> my_last_camframe;
        };
 
 
@@ -445,7 +449,7 @@
   return TooN::Vector<2>(scale * diagmult(camframe, 
my_camera_parameters.slice<0,2>()) + my_camera_parameters.slice<2,2>());
 }
 
-inline TooN::Vector<2> Camera::Cubic::project(const TooN::Vector<2>& camframe){
+inline TooN::Vector<2> Camera::Cubic::project(const TooN::Vector<2>& camframe) 
const{
   my_last_camframe = camframe;
   TooN::Vector<2> mod_camframe = camframe * 
(1+SAT(my_camera_parameters[4]*(camframe*camframe)));
   return TooN::Vector<2>(diagmult(mod_camframe, 
my_camera_parameters.slice<0,2>()) + my_camera_parameters.slice<2,2>());

Index: progs/calibrate.cxx
===================================================================
RCS file: /cvsroot/libcvd/libcvd/progs/calibrate.cxx,v
retrieving revision 1.8
retrieving revision 1.9
diff -u -b -r1.8 -r1.9
--- progs/calibrate.cxx 28 Aug 2008 18:23:08 -0000      1.8
+++ progs/calibrate.cxx 29 Aug 2008 03:33:26 -0000      1.9
@@ -41,7 +41,21 @@
 #else
 string videoDevice = "v4l2:///dev/video0";
 #endif
-Vector<6> cameraParameters = (make_Vector, -1, -1, -1, -1, -1, -1);
+
+typedef Camera::Quintic CameraModel;
+static const int NumCameraParams = CameraModel::num_parameters;
+
+Vector<NumCameraParams> badVector()
+{
+       Vector<NumCameraParams> v;
+       for(int i=0; i < NumCameraParams; i++)
+               v[i] = -1;
+
+       return v;
+}
+
+
+Vector<NumCameraParams> cameraParameters = badVector();
 int bottomLeftWhite = 1;
 int gridx = 11;
 int gridy = 7;
@@ -93,11 +107,14 @@
 }
 
 template <class CamModel, class P>
-SE3 find_pose(const SE3& start, const vector<Vector<3> >& x, const 
vector<pair<size_t,P> >& p, const CamModel& camModel, double noise)
+SE3 find_pose(const SE3& start, const vector<Vector<3> >& x, const 
vector<pair<size_t,P> >& p, CamModel& camModel, double noise)
 {
     vector<pair<Vector<2>, Matrix<2> > > unprojected(p.size());
+
     for (size_t i=0; i<p.size(); ++i) {
-       unprojected[i] = camModel.unproject(p[i].second, Identity<2>(noise));
+       unprojected[i].first = camModel.unproject(p[i].second);
+       Matrix<2> Jinv = inverse(camModel.get_derivative());
+       unprojected[i].second = Jinv * Identity<2>(noise) * Jinv.T();
     }
 
     SE3 bestSE3;
@@ -260,7 +277,7 @@
 }
 
 template <class CM>
-void drawPoints(const vector<Vector<2> >& points, const SE3& pose, const CM& 
cm)
+void drawPoints(const vector<Vector<2> >& points, const SE3& pose, CM& cm)
 {
     glColor3f(0,1,0);
     for (size_t i=0; i<points.size(); i++)
@@ -340,7 +357,7 @@
     Vector<> JTe(JTJ.num_rows());
 
     Zero(JTJ);
-    Vector<CM::num_parameters> JTep = zeros<6>();
+    Vector<CM::num_parameters> JTep = zeros<CM::num_parameters>();
 
     for (size_t i=0; i<ms.size(); i++)
     {
@@ -389,7 +406,7 @@
 }
 
 template <class CM>
-void getUncertainty(const vector<MeasurementSet>& ms, const vector<SE3>& pose, 
const CM& cm, Matrix<CM::num_parameters>& C)
+void getUncertainty(const vector<MeasurementSet>& ms, const vector<SE3>& pose, 
CM& cm, Matrix<CM::num_parameters>& C)
 {
     Matrix<> 
JTJ(CM::num_parameters+ms.size()*6,CM::num_parameters+ms.size()*6);
     Zero(JTJ);
@@ -430,7 +447,7 @@
     }
 }
 
-inline Vector<2> imagePoint(const Vector<2>& inPoint, const SE3& pose, const 
Camera::Quintic& cm, const double& factor)
+inline Vector<2> imagePoint(const Vector<2>& inPoint, const SE3& pose, 
CameraModel& cm, const double& factor)
 {
     Vector<3> point3D = unproject(inPoint);
     Vector<2> plane = project(pose*point3D);
@@ -440,7 +457,7 @@
 }
 
 inline float imageVal(image_interpolate<Interpolate::Bilinear, float> 
&imgInter, const Vector<2>& inPoint, const SE3& pose,
-                      const Camera::Quintic& cm, const double& factor, const 
bool& boundsCheck)
+                      CameraModel& cm, const double& factor, const bool& 
boundsCheck)
 {
     Vector<2> imageVec = imagePoint(inPoint, pose, cm, factor);
     if(!boundsCheck) //Slight speed-up
@@ -482,7 +499,7 @@
 }
 
 inline float minMarginSquare(const Vector<2>& inPoint, 
image_interpolate<Interpolate::Bilinear, float>& imgInter, const SE3& pose,
-                             const Camera::Quintic& cm, double factor, float 
cellSize)
+                             CameraModel& cm, double factor, float cellSize)
 {
     Vector<2> posVec = (make_Vector, cellSize/2, cellSize/2);
     Vector<2> negVec = (make_Vector, cellSize/2, -cellSize/2);
@@ -500,7 +517,7 @@
 }
 
 bool sanityCheck(const Vector<2>& inPoint, 
image_interpolate<Interpolate::Bilinear, float>& imgInter, const SE3& pose,
-                 const Camera::Quintic& cm, double factor, bool blWhite, float 
cellSize)
+                 CameraModel& cm, double factor, bool blWhite, float cellSize)
 {
     Vector<2> posVec = (make_Vector, cellSize/2, cellSize/2);
     Vector<2> negVec = (make_Vector, cellSize/2, -cellSize/2);
@@ -539,7 +556,7 @@
 }
 
 bool findInitialIntersectionEstimate(image_interpolate<Interpolate::Bilinear, 
float> &imgInter, Vector<2>& initialPoint,
-                                     const SE3& pose, const Camera::Quintic& 
cm, double factor,
+                                     const SE3& pose, CameraModel& cm, double 
factor,
                                      bool boundsCheck, const Vector<4>& 
likelySquare, double cellSize)
 {
     bool looksOK = true;
@@ -570,7 +587,7 @@
 }
 
 bool optimiseIntersection(image_interpolate<Interpolate::Bilinear, float> 
&imgInter, Vector<2>& inPoint,
-                          const SE3& pose, const Camera::Quintic& cm, double 
factor, bool boundsCheck,
+                          const SE3& pose, CameraModel& cm, double factor, 
bool boundsCheck,
                           const Vector<4>& likelySquare, double cellSize, bool 
blWhite)
 {
     Vector<2> largeX = (make_Vector, 0.004, 0);
@@ -645,7 +662,7 @@
     GLWindow disp(videoBuffer->size(), titlePrefix);
     GLWindow::EventSummary events;
     //VideoDisplay disp (0.0, 0.0, 640.0, 480.0);
-    Camera::Quintic cameraModel;
+    CameraModel cameraModel;
     double factor=1.0;
     vector<MeasurementSet> measurementSets;
     vector<SE3> poses;
@@ -1025,7 +1042,7 @@
     double lambda = 1;
     while (lambda < 1e12)
     {
-       Vector<6> params = cameraModel.get_parameters();
+       Vector<NumCameraParams> params = cameraModel.get_parameters();
        vector<SE3> oldposes = poses;
        improveLM(measurementSets, poses, cameraModel, lambda);
        double error = getReprojectionError(measurementSets, poses, 
cameraModel);
@@ -1063,14 +1080,15 @@
     }
 
     cerr << "Estimating uncertainty..." << endl;
-    Matrix<6> Cov;
+    Matrix<NumCameraParams> Cov;
     getUncertainty(measurementSets, poses, cameraModel, Cov);
-    Cov.slice<4,4,2,2>() *= factor*factor;
-    Cov.slice<0,4,4,2>() *= factor;
-    Cov.slice<4,0,2,4>() *= factor;
+    static const int NumRadialParams = NumCameraParams - 4;
+    Cov.slice<4,4,NumRadialParams,NumRadialParams>() *= factor*factor;
+    Cov.slice<0,4,4,NumRadialParams>() *= factor;
+    Cov.slice<4,0,NumRadialParams,4>() *= factor;
 
-    Vector<6> uncertainty;
-    for (size_t i=0; i<6; i++)
+    Vector<NumCameraParams> uncertainty;
+    for (int i=0; i<NumCameraParams; i++)
        uncertainty[i]= 3*sqrt(Cov[i][i]);
 
     cameraModel.get_parameters().slice<0,4>() /= factor;