Update equations number related to the paper last version

.gitignore

@@ -1,2 +1,5 @@
-svn-commit.tmp*
 *~
+*/CMakeFiles
+CMakeCache.txt
+.DS_Store
+CMakeLists.txt.user*

doc/tutorial-homography-opencv.doc

@@ -26,7 +26,7 @@ From a vector of planar points \f${\bf x_1} = (x_1, y_1, 1)^T\f$ in image \f$I_1
 
 \f[\bf x_2 = {^2}H_1 x_1\f]
 
-The implementation of the Direct Linear Transform algorithm to estimate \f$\bf {^2}H_1\f$ is done next. First, for each point we update the values of matrix A using equation (33). Then we solve the system \f${\bf Ah}=0\f$ using a Singular Value Decomposition of \f$\bf A\f$. Finaly, we determine the smallest eigen value that allows to identify the eigen vector that corresponds to the solution \f$\bf h\f$.
+The implementation of the Direct Linear Transform algorithm to estimate \f$\bf {^2}H_1\f$ is done next. First, for each point we update the values of matrix A using equation (23). Then we solve the system \f${\bf Ah}=0\f$ using a Singular Value Decomposition of \f$\bf A\f$. Finaly, we determine the smallest eigen value that allows to identify the eigen vector that corresponds to the solution \f$\bf h\f$.
 
 \snippet homography-dlt-opencv.cpp DLT
 

doc/tutorial-homography-visp.doc

@@ -26,7 +26,7 @@ From a vector of planar points \f${\bf x_1} = (x_1, y_1, 1)^T\f$ in image \f$I_1
 
 \f[\bf x_2 = {^2}H_1 x_1\f]
 
-The implementation of the Direct Linear Transform algorithm to estimate \f$\bf {^2}H_1\f$ is done next. First, for each point we update the values of matrix A using equation (33). Then we solve the system \f${\bf Ah}=0\f$ using a Singular Value Decomposition of \f$\bf A\f$. Finaly, we determine the smallest eigen value that allows to identify the eigen vector that corresponds to the solution \f$\bf h\f$.
+The implementation of the Direct Linear Transform algorithm to estimate \f$\bf {^2}H_1\f$ is done next. First, for each point we update the values of matrix A using equation (23). Then we solve the system \f${\bf Ah}=0\f$ using a Singular Value Decomposition of \f$\bf A\f$. Finaly, we determine the smallest eigen value that allows to identify the eigen vector that corresponds to the solution \f$\bf h\f$.
 
 \snippet homography-dlt-visp.cpp DLT
 

doc/tutorial-pose-dlt-planar-opencv.doc

@@ -26,7 +26,7 @@ Then we introduce the function that does the homography estimation from coplanar
 Then we introduce the function that does the pose from homography estimation.
 \snippet pose-from-homography-dlt-opencv.cpp Estimation function
 
-Based on equation (37) \f${\bf x}_0 = {^0}{\bf H}_w {\bf x}_w\f$ we first estimate the homography \f${^0}{\bf H}_w\f$.
+Based on equation (27) \f${\bf x}_0 = {^0}{\bf H}_w {\bf x}_w\f$ we first estimate the homography \f${^0}{\bf H}_w\f$.
 \snippet pose-from-homography-dlt-opencv.cpp Homography estimation
 
 Then using the constraint that \f$||{\bf c}^0_1|| = 1\f$ we normalize the homography.
@@ -56,7 +56,7 @@ Then we create the data structures that will contain the 3D points coordinates \
 \snippet pose-from-homography-dlt-opencv.cpp Create data structures
 
 For our simulation we then initialize the input data from a ground truth pose with the translation in \e ctw_truth and the rotation matrix in \e cRw_truth. 
-For each point \e wX[i] we compute the perspective projection \e xo[i] = (xo, yo, 1). According to equation (37) we also set \f${\bf x}_w = ({^w}X, {^w}Y, 1)\f$ in \e xw vector.
+For each point \e wX[i] we compute the perspective projection \e xo[i] = (xo, yo, 1). According to equation (27) we also set \f${\bf x}_w = ({^w}X, {^w}Y, 1)\f$ in \e xw vector.
 \snippet pose-from-homography-dlt-opencv.cpp Simulation
 
 From here we have initialized \f${\bf x_0} = (x_0,y_0,1)^T\f$ and \f${\bf x}_w = ({^w}X, {^w}Y, 1)^T\f$. We are now ready to call the function that does the pose estimation.

doc/tutorial-pose-dlt-planar-visp.doc

@@ -26,7 +26,7 @@ Then we introduce the function that does the homography estimation from coplanar
 Then we introduce the function that does the pose from homography estimation.
 \snippet pose-from-homography-dlt-visp.cpp Estimation function
 
-Based on equation (37) \f${\bf x}_0 = {^0}{\bf H}_w {\bf x}_w\f$ we first estimate the homography \f${^0}{\bf H}_w\f$.
+Based on equation (27) \f${\bf x}_0 = {^0}{\bf H}_w {\bf x}_w\f$ we first estimate the homography \f${^0}{\bf H}_w\f$.
 \snippet pose-from-homography-dlt-visp.cpp Homography estimation
 
 Then using the constraint that \f$||{\bf c}^0_1|| = 1\f$ we normalize the homography.
@@ -56,7 +56,7 @@ Then we create the data structures that will contain the 3D points coordinates \
 \snippet pose-from-homography-dlt-visp.cpp Create data structures
 
 For our simulation we then initialize the input data from a ground truth pose \e oTw_truth. 
-For each point \e wX[i] we compute the perspective projection \e xo[i] = (xo, yo, 1). According to equation (37) we also set \f${\bf x}_w = ({^w}X, {^w}Y, 1)\f$ in \e xw vector.
+For each point \e wX[i] we compute the perspective projection \e xo[i] = (xo, yo, 1). According to equation (27) we also set \f${\bf x}_w = ({^w}X, {^w}Y, 1)\f$ in \e xw vector.
 \snippet pose-from-homography-dlt-visp.cpp Simulation
 
 From here we have initialized \f${\bf x_0} = (x_0,y_0,1)^T\f$ and \f${\bf x}_w = ({^w}X, {^w}Y, 1)^T\f$. We are now ready to call the function that does the pose estimation.

opencv/homography-basis/homography-dlt-opencv.cpp

@@ -21,7 +21,7 @@ cv::Mat homography_dlt(const std::vector< cv::Point2d > &x1, const std::vector<
 
   // Since the third line of matrix A is a linear combination of the first and second lines
   // (A is rank 2) we don't need to implement this third line
-  for(int i = 0; i < npoints; i++) {              // Update matrix A using eq. 33
+  for(int i = 0; i < npoints; i++) {              // Update matrix A using eq. 23
     A.at<double>(2*i,3) = -x1[i].x;               // -xi_1
     A.at<double>(2*i,4) = -x1[i].y;               // -yi_1
     A.at<double>(2*i,5) = -1;                     // -1

opencv/homography-basis/pose-from-homography-dlt-opencv.cpp

@@ -21,7 +21,7 @@ cv::Mat homography_dlt(const std::vector< cv::Point2d > &x1, const std::vector<
 
   // Since the third line of matrix A is a linear combination of the first and second lines
   // (A is rank 2) we don't need to implement this third line
-  for(int i = 0; i < npoints; i++) {      // Update matrix A using eq. 33
+  for(int i = 0; i < npoints; i++) {              // Update matrix A using eq. 23
     A.at<double>(2*i,3) = -x1[i].x;               // -xi_1
     A.at<double>(2*i,4) = -x1[i].y;               // -yi_1
     A.at<double>(2*i,5) = -1;                     // -1

opencv/pose-basis/pose-dementhon-opencv.cpp

@@ -16,7 +16,7 @@ void pose_dementhon(const std::vector< cv::Point3d > &wX,
   int npoints = (int)wX.size();
   cv::Mat r1, r2, r3;
   cv::Mat A(npoints, 4, CV_64F);
-  for(int i = 0; i < npoints; i++) { // Equation (12)
+  for(int i = 0; i < npoints; i++) {
       A.at<double>(i,0) = wX[i].x;
       A.at<double>(i,1) = wX[i].y;
       A.at<double>(i,2) = wX[i].z;
@@ -34,11 +34,11 @@ void pose_dementhon(const std::vector< cv::Point3d > &wX,
   // POSIT loop
   for(unsigned int iter = 0; iter < 20; iter ++) {
     for(int i = 0; i < npoints; i++) {
-      Bx.at<double>(i,0) = x[i].x * (eps.at<double>(i,0) + 1.); // Equation (13)
-      By.at<double>(i,0) = x[i].y * (eps.at<double>(i,0) + 1.); // Equation (14)
+      Bx.at<double>(i,0) = x[i].x * (eps.at<double>(i,0) + 1.);
+      By.at<double>(i,0) = x[i].y * (eps.at<double>(i,0) + 1.);
     }
 
-    I = Ap * Bx; // Equation (15). Notice that the pseudo inverse
+    I = Ap * Bx; // Notice that the pseudo inverse
     J = Ap * By; // of matrix A is a constant that has been precompiled.
 
     for (int i = 0; i < 3; i++) {
@@ -54,7 +54,7 @@ void pose_dementhon(const std::vector< cv::Point3d > &wX,
     }
     normI = sqrt(normI);
     normJ = sqrt(normJ);
-    r1 = Istar / normI; // Equation (16)
+    r1 = Istar / normI;
     r2 = Jstar / normJ;
     r3 = r1.cross(r2);
 

opencv/pose-basis/pose-dlt-opencv.cpp

@@ -14,7 +14,7 @@ void pose_dlt(const std::vector< cv::Point3d > &wX, const std::vector< cv::Point
   int npoints = (int)wX.size();
   cv::Mat A(2*npoints, 12, CV_64F, cv::Scalar(0));
   for(int i = 0; i < npoints; i++) { // Update matrix A using eq. 5
-    A.at<double>(2*i, 0) = wX[i].x; //wX[i][0] ;
+    A.at<double>(2*i, 0) = wX[i].x;
     A.at<double>(2*i, 1) = wX[i].y;
     A.at<double>(2*i, 2) = wX[i].z;
     A.at<double>(2*i, 3) = 1 ;

opencv/pose-basis/pose-gauss-newton-opencv.cpp

@@ -65,10 +65,10 @@ void pose_gauss_newton(const std::vector< cv::Point3d > &wX,
     for (int i = 0; i < npoints; i++) {
       cX = cRw * cv::Mat(wX[i]) + ctw;                      // Update cX, cY, cZ
       // Update x(q)
-      xq.at<double>(i*2,0)   = cX.at<double>(0,0) / cX.at<double>(2,0);                    // x(q) = cX/cZ
-      xq.at<double>(i*2+1,0) = cX.at<double>(1,0) / cX.at<double>(2,0);                    // y(q) = cY/cZ
+      xq.at<double>(i*2,0)   = cX.at<double>(0,0) / cX.at<double>(2,0); // x(q) = cX/cZ
+      xq.at<double>(i*2+1,0) = cX.at<double>(1,0) / cX.at<double>(2,0); // y(q) = cY/cZ
 
-      // Update J using equation (22)
+      // Update J using equation (11)
       J.at<double>(i*2,0) = -1/cX.at<double>(2,0);          // -1/cZ
       J.at<double>(i*2,1) = 0;
       J.at<double>(i*2,2) = x[i].x / cX.at<double>(2,0);    // x/cZ
@@ -84,10 +84,10 @@ void pose_gauss_newton(const std::vector< cv::Point3d > &wX,
       J.at<double>(i*2+1,5) = -x[i].y;                      // -x
     }
 
-    cv::Mat e_q = xq - xn;                                  // Equation (18)
+    cv::Mat e_q = xq - xn;                                  // Equation (7)
 
     cv::Mat Jp = J.inv(cv::DECOMP_SVD);                     // Compute pseudo inverse of the Jacobian
-    cv::Mat dq = -lambda * Jp * e_q;                        // Equation (21)
+    cv::Mat dq = -lambda * Jp * e_q;                        // Equation (10)
 
     cv::Mat dctw(3,1,CV_64F), dcRw(3,3,CV_64F);
     exponential_map(dq, dctw, dcRw);

visp/homography-basis/homography-dlt-visp.cpp

@@ -22,7 +22,7 @@ vpMatrix homography_dlt(const std::vector< vpColVector > &x1, const std::vector<
 
   // Since the third line of matrix A is a linear combination of the first and second lines
   // (A is rank 2) we don't need to implement this third line
-  for(int i = 0; i < npoints; i++) {      // Update matrix A using eq. 33
+  for(int i = 0; i < npoints; i++) {      // Update matrix A using eq. 23
     A[2*i][3] = -x1[i][0];                // -xi_1
     A[2*i][4] = -x1[i][1];                // -yi_1
     A[2*i][5] = -x1[i][2];                // -1

visp/homography-basis/pose-from-homography-dlt-visp.cpp

@@ -21,7 +21,7 @@ vpMatrix homography_dlt(const std::vector< vpColVector > &x1, const std::vector<
 
   // Since the third line of matrix A is a linear combination of the first and second lines
   // (A is rank 2) we don't need to implement this third line
-  for(int i = 0; i < npoints; i++) {      // Update matrix A using eq. 33
+  for(int i = 0; i < npoints; i++) {      // Update matrix A using eq. 23
     A[2*i][3] = -x1[i][0];                // -xi_1
     A[2*i][4] = -x1[i][1];                // -yi_1
     A[2*i][5] = -x1[i][2];                // -1

visp/pose-basis/pose-dementhon-visp.cpp

@@ -16,7 +16,7 @@ vpHomogeneousMatrix pose_dementhon(const std::vector< vpColVector > &wX, const s
   vpMatrix A(npoints, 4);
   for(int i = 0; i < npoints; i++) {
     for (int j = 0; j < 4; j++) {
-      A[i][j] = wX[i][j]; // Equation (12)
+      A[i][j] = wX[i][j];
     }
   }
   vpMatrix Ap = A.pseudoInverse();
@@ -33,11 +33,11 @@ vpHomogeneousMatrix pose_dementhon(const std::vector< vpColVector > &wX, const s
   // POSIT loop
   for(unsigned int iter = 0; iter < 20; iter ++) {
     for(int i = 0; i < npoints; i++) {
-      Bx[i] = x[i][0] * (eps[i] + 1.); // Equation (13)
-      By[i] = x[i][1] * (eps[i] + 1.); // Equation (14)
+      Bx[i] = x[i][0] * (eps[i] + 1.);
+      By[i] = x[i][1] * (eps[i] + 1.);
     }
 
-    I = Ap * Bx; // Equation (15). Notice that the pseudo inverse
+    I = Ap * Bx; // Notice that the pseudo inverse
     J = Ap * By; // of matrix A is a constant that has been precompiled.
 
     for (int i = 0; i < 3; i++) {
@@ -48,7 +48,7 @@ vpHomogeneousMatrix pose_dementhon(const std::vector< vpColVector > &wX, const s
     // Estimation of the rotation matrix
     double normI = sqrt( Istar.sumSquare() );
     double normJ = sqrt( Jstar.sumSquare() );
-    r1 = Istar / normI; // Equation (16)
+    r1 = Istar / normI;
     r2 = Jstar / normJ;
     r3 = vpColVector::crossProd(r1, r2);
 

visp/pose-basis/pose-gauss-newton-visp.cpp

@@ -36,7 +36,7 @@ vpHomogeneousMatrix pose_gauss_newton(
       xq[i*2]   = cX[0] / cX[2];                          // x(q) = cX/cZ
       xq[i*2+1] = cX[1] / cX[2];                          // y(q) = cY/cZ
 
-      // Update J using equation (22)
+      // Update J using equation (11)
       J[i*2][0] = -1/cX[2];                               // -1/cZ
       J[i*2][1] = 0;
       J[i*2][2] = x[i][0] / cX[2];                        // x/cZ
@@ -52,10 +52,10 @@ vpHomogeneousMatrix pose_gauss_newton(
       J[i*2+1][5] = -x[i][1];                             // -x
     }
 
-    vpColVector e_q = xq - xn;                            // Equation (18)
+    vpColVector e_q = xq - xn;                            // Equation (7)
 
     J.pseudoInverse(Jp);                                  // Compute pseudo inverse of the Jacobian
-    vpColVector dq = -lambda * Jp * e_q;                  // Equation (21)
+    vpColVector dq = -lambda * Jp * e_q;                  // Equation (10)
 
     cTw_ = vpExponentialMap::direct(dq).inverse() * cTw_; // Update the pose