debug v3.2

CMakeLists.txt

@@ -94,7 +94,7 @@ include_directories(
 
 #add_library(cuda_prof_lib ${cuda_PROFILER})
 
-if(CUDA_FOUND)
+if(!CUDA_FOUND)
   set(vi_slam_GPU_SOURCE_FILES
       ${PROJECT_SOURCE_DIR}/src/MatcherGPU.cpp 
       ${PROJECT_SOURCE_DIR}/src/CameraGPU.cpp 
@@ -103,7 +103,7 @@ if(CUDA_FOUND)
   add_library(vi_slam_libs ${vi_slam_SOURCE_FILES} ${vi_slam_GPU_SOURCE_FILES}) # comentado hasta que se cree librerias
   add_executable(vi_slam ${PROJECT_SOURCE_DIR}/src/main_vi_slamGPU.cpp)
   target_link_libraries(vi_slam vi_slam_libs ${OpenCV_LIBS} ${catkin_LIBRARIES} ${CUDA_LIBRARIES})
-  else(CUDA_FOUND)
+  else(!CUDA_FOUND)
     message("******************")
     message("CUDA not found")
     message("vi_slam CPU version will be compiled")

include/VISystem.hpp

@@ -54,7 +54,7 @@ class VISystem
         Mat IdentityWeights(int _num_residuals) ;
         Mat TukeyFunctionWeights(Mat _input) ;
         Mat WarpFunctionSE3(Mat _points2warp, SE3 _rigid_transformation, int _lvl);
-        void WarpFunctionRT(vector <KeyPoint> inPoints, Mat rotationMat, Mat translationMat, vector <KeyPoint> &outPoints, float z);
+        void WarpFunctionRT(vector <KeyPoint> inPoints, Mat rotationMat, Mat translationMat, vector <KeyPoint> &outPoints);
         float MedianAbsoluteDeviation(Mat _input);
         float MedianMat(Mat _input) ;
 

launch/vi_slam.launch

@@ -2,9 +2,9 @@
 <launch>
     <!-- VI-SLAM main thread-->
   <node pkg="vi_slam" type="vi_slam" name="main"  
-        args="-gtFile=/media/lujano/fc309465-b35b-4d2f-bde1-c379f1cbf3f3/home/lujano/Documents/V1_01_easy/mav0/state_groundtruth_estimate0/data.csv 
-              -imuFile=/media/lujano/fc309465-b35b-4d2f-bde1-c379f1cbf3f3/home/lujano/Documents/V1_01_easy/mav0/imu0/data.csv 
-              -imagesPath=/media/lujano/fc309465-b35b-4d2f-bde1-c379f1cbf3f3/home/lujano/Documents/V1_01_easy/mav0/cam0/data/
+        args="-gtFile=/home/lujano/Documents/V1_01_easy/mav0/state_groundtruth_estimate0/data.csv 
+              -imuFile=/home/lujano/Documents/V1_01_easy/mav0/imu0/data.csv 
+              -imagesPath=/home/lujano/Documents/V1_01_easy/mav0/cam0/data/
               -calibrationFile=/home/lujano/catkin_ws/src/vi-slam/calibration/calibrationEUROC.xml
               -outputFile=/home/lujano/Documents/ouput.txt"
         clear_params="true" 

src/DataReader.cpp

@@ -82,6 +82,9 @@ void DataReader::setProperties(string image_path, string imu_path, string gt_pat
     cout<<"index camera = "<<imageIndex0<<endl;
     cout<<"index imu = "<<imuIndex0<<endl;
     cout<<"index get ="<<gtIndex0<<endl;
+    cout<< "Time first image = " << timeFirstImage<<endl;
+    cout<< "Time first gt = " << timeFirstLineGt<<endl;
+    cout<< "Time dif = " << timeFirstLineGt-timeFirstImage<<endl;
 
 
 
@@ -116,6 +119,8 @@ void DataReader::UpdateDataReader(int index, int index2){
     double timeImage = imageReader.getImageTime(imageIndex0+index); // poner en clase
     double timeLineImu = imuReader.getGroundTruthData(indexImuReader, 0);
     double timeLineGt= gtReader.getGroundTruthData(indexGtReader, 0);
+    cout << "time dif " << timeImage -timeLineGt <<endl;
+
     double delta1;
     double delta2;
     // Encontrar el indice del dato de gt mas cercano a la imagen
@@ -126,6 +131,7 @@ void DataReader::UpdateDataReader(int index, int index2){
         timeLineGt= gtReader.getGroundTruthData(indexGtReader, 0);
         delta1 = timeLineGt-timeImage;
     }
+    cout << "time dif " << timeImage -timeLineGt <<endl;
      // Encontrar el indice del dato de imu posterior a la imagen1    
 
     delta2 = timeLineImu-timeImage;
@@ -154,6 +160,9 @@ void DataReader::UpdateDataReader(int index, int index2){
      gtRPY.clear();
      accBias.clear();
     // Obtener todos los datos entre las dos imagenes
+    timeImuReader = imuReader.getGroundTruthData(indexImuReader, 0);
+    cout << "time image 2 = " << imageReader.getImageTime(imageIndex0 +index2) - imageReader.getImageTime(imageIndex0 +index)<<endl;
+        cout << "time imu reader" << imageReader.getImageTime(imageIndex0 +index)-timeImuReader<<endl;
     while(timeImuReader<=static_cast<double>(imageReader.getImageTime(imageIndex0 +index2)))
     {
         angularVelocity.x =  imuReader.getGroundTruthData(indexImuReader, 1);
@@ -170,14 +179,18 @@ void DataReader::UpdateDataReader(int index, int index2){
         indexImuReader++;
         timeImuReader = imuReader.getGroundTruthData(indexImuReader, 0);
     }
-
+    //cout << "time imu reader2" << imageReader.getImageTime(imageIndex0 +index2)-timeImuReader<<endl;
     // Tomar los datos del primer bias acc y ang
     angBias.x = gtReader.getGroundTruthData(indexGtReader, 11);
     angBias.y = gtReader.getGroundTruthData(indexGtReader, 12);
     angBias.z = gtReader.getGroundTruthData(indexGtReader, 13);
 
     Point3d biasAcc;
     // tomar el resto de los datos de gt
+    indexGtReader++;
+    timeGtReader = gtReader.getGroundTruthData(indexGtReader, 0);
+    //cout << "time gt reader" << imageReader.getImageTime(imageIndex0 +index)<<endl;
+
     while(timeGtReader<=static_cast<double>(imageReader.getImageTime(imageIndex0 +index2)))
     {
 
@@ -190,6 +203,8 @@ void DataReader::UpdateDataReader(int index, int index2){
         quaternion.y = gtReader.getGroundTruthData(indexGtReader, 6);
         quaternion.z = gtReader.getGroundTruthData(indexGtReader, 7);
 
+        //cout << "q " << quaternion.x << "p " << position.x << endl;
+
         rpy = toRPY(quaternion);
 
         linear_velocity.x = gtReader.getGroundTruthData(indexGtReader, 8);
@@ -215,6 +230,7 @@ void DataReader::UpdateDataReader(int index, int index2){
         timeGtReader = gtReader.getGroundTruthData(indexGtReader, 0);
     }
 
+    //cout << "time gt reader" << imageReader.getImageTime(imageIndex0 +index2)<<endl;
     //cout <<"ImuIndex2 "<<indexImuReader<<endl;
     //cout <<"GtIndex2 "<<indexGtReader<<endl;
 

src/Plus.cpp

@@ -298,8 +298,7 @@ Mat RPYAndPosition2transformationMatrix(Point3d rpy, Point3d position)
     double s3 = sin(yaw);
 
 	Mat transformationMatrix = Mat::zeros(4,4,CV_32FC1);
-
-
+
 	transformationMatrix.at<float>(0,0) = c3*c2;
 	transformationMatrix.at<float>(0,1) = c3*s2*s1-s3*c1;
 	transformationMatrix.at<float>(0,2) = c3*s2*c1+s3*s1;

src/VISystem.cpp

@@ -327,7 +327,7 @@ namespace vi
     void VISystem::setGtRes(Mat TraslationResGT, Mat RotationResGT )
     {
         TraslationResidual = TraslationResGT;
-        RotationResidual = RotationResGT;
+        RotationResidual =  RPY2rotationMatrix (rotationMatrix2RPY(RotationResGT)) ;
     }
 
 
@@ -354,23 +354,24 @@ namespace vi
 
 
         vector<KeyPoint> warpedDebugKeyPoints;
-        vector<KeyPoint> warpedDebugKeyPoints2;
-        WarpFunctionRT(_current_frame->prevGoodMatches, RotationResidual, TraslationResidual, warpedDebugKeyPoints, 1.0);
-        WarpFunctionRT(_current_frame->prevGoodMatches, RotationResidual, TraslationResidual, warpedDebugKeyPoints2, 10.0);
-        //WarpFunctionRT(_current_frame->prevGoodMatches, Mat::eye(3, 3, CV_32FC1), Mat::zeros(3, 1, CV_32FC1), warpedDebugKeyPoints);
+         Mat trakity = Mat::zeros(3, 1, CV_32FC1);
+         trakity.at<float>(2,0)= 2.0;
+
+        WarpFunctionRT(_current_frame->prevGoodMatches, RotationResidual, TraslationResidual, warpedDebugKeyPoints);
+        //WarpFunctionRT(_previous_frame->nextGoodMatches, Mat::eye(3, 3, CV_32FC1), trakity, warpedDebugKeyPoints);
 
          // WarpFunctionRT(_current_frame->prevGoodMatches, Mat::eye(3, 3, CV_32FC1), Mat::zeros(3, 1, CV_32FC1), warpedDebugKeyPoints, 1.0);
          //WarpFunctionRT(_current_frame->prevGoodMatches, Mat::eye(3, 3, CV_32FC1), Mat::zeros(3, 1, CV_32FC1), warpedDebugKeyPoints2, 10);
 
         float u1 = warpedDebugKeyPoints[0].pt.x;
-        float    v1 = warpedDebugKeyPoints[0].pt.y;
-                float u2 = warpedDebugKeyPoints2[0].pt.x;
-        float    v2 = warpedDebugKeyPoints2[0].pt.y;
+        float v1 = warpedDebugKeyPoints[0].pt.y;
+        float u2 = _previous_frame->nextGoodMatches[0].pt.x;
+        float v2 = _previous_frame->nextGoodMatches[0].pt.y;
         cout << "u1 " << u1 << " v1 " <<  v1 << " u2 " << u2<< " v2 " << v2 <<endl;
 
         drawKeypoints(_previous_frame->grayImage[lvl], warpedDebugKeyPoints, currentImageDebugToShow, Scalar(0,255, 0), DrawMatchesFlags::DEFAULT);
-        drawKeypoints(currentImageDebugToShow, warpedDebugKeyPoints2, currentImageDebugToShow, Scalar(0,0, 255), DrawMatchesFlags::DEFAULT);
         drawKeypoints(currentImageDebugToShow, _previous_frame->nextGoodMatches, currentImageDebugToShow, Scalar(255,0, 0), DrawMatchesFlags::DEFAULT);
+        drawKeypoints(currentImageDebugToShow, _current_frame->prevGoodMatches, currentImageDebugToShow, Scalar(0,0, 255), DrawMatchesFlags::DEFAULT);
         drawKeypoints(_current_frame->grayImage[lvl], _current_frame->prevGoodMatches, currentImageToShow, Scalar(255,0, 0), DrawMatchesFlags::DEFAULT);
 
         imshow("currentDebugImage1", currentImageDebugToShow);
@@ -384,7 +385,7 @@ namespace vi
     }
 
 
-    void VISystem::WarpFunctionRT(vector <KeyPoint> inPoints, Mat rotationMat, Mat translationMat, vector <KeyPoint> &outPoints, float z)
+    void VISystem::WarpFunctionRT(vector <KeyPoint> inPoints, Mat rotationMat, Mat translationMat, vector <KeyPoint> &outPoints)
     {
         Mat cameraMatrix = Mat::zeros(3,3,CV_32F);
         cameraMatrix.at<float>(0, 0) = fx_[0];
@@ -399,45 +400,56 @@ namespace vi
         float fy = fy_[lvl];
         float cx = cx_[lvl];
         float cy = cy_[lvl];
-
-        Mat projectionMat;
-        hconcat(rotationMat, translationMat, projectionMat);
-        cout << projectionMat <<endl;
+        Mat projectionMat, transformationMat ;
+        hconcat(rotationMat, translationMat, transformationMat);
+        //cout << transformationMat<<endl;
 
-        projectionMat = cameraMatrix * projectionMat;
+        //projectionMat = cameraMatrix * transformationMat;
 
-        cout << projectionMat <<endl;
+        //cout << projectionMat <<endl;
 
 
         int numkeypoints = inPoints.size();
 
         float u1, v1, u2, v2, a, b;
-        Mat inPoint3dHomo = Mat::ones(4, 1, CV_32FC1); 
+        Mat inPoint3dHomo = Mat::ones(3, 1, CV_32FC1); 
         Mat outPoint3dHomo = Mat::ones(3, 1, CV_32FC1); 
 
-        for (int index = 0; index< numkeypoints; index++)
+        for (int index = 0; index< numkeypoints ; index++)
         {
             u1 = inPoints[index].pt.x;
             v1 = inPoints[index].pt.y;
-            a = z*(u1-cx)/fx;
-            b = z*(v1-cy)/fy;
+            a = (u1-cx)/fx;
+            b = (v1-cy)/fy;
             inPoint3dHomo.at<float>(0, 0) = a;
             inPoint3dHomo.at<float>(1, 0) = b;
-            inPoint3dHomo.at<float>(2, 0) = z;
+
+            //inPoint3dHomo.at<float>(2, 0) = 1.0; // z = 1.0
+
+            //cout << "transf "<<  " i " << index << " "<< transformationMat*inPoint3dHomo <<endl;
+            //cout << "transf "<<  " i " << index << " "<< transformationMat*inPoint3dHomo*z <<endl;
+            outPoint3dHomo = rotationMat*inPoint3dHomo +translationMat;
 
-            outPoint3dHomo = projectionMat*inPoint3dHomo;
 
             KeyPoint outKeypoint;
-            float z2 = outPoint3dHomo.at<float>(2, 0)/z;
             //cout << a << "fdas " << b <<  " z " << endl;
-            outKeypoint.pt.x = outPoint3dHomo.at<float>(0, 0)/(z);
-            outKeypoint.pt.y = outPoint3dHomo.at<float>(1, 0)/(z);
+            outKeypoint.pt.x = (fx*outPoint3dHomo.at<float>(0, 0))/outPoint3dHomo.at<float>(2, 0) +cx;
+            outKeypoint.pt.y =  (fy*outPoint3dHomo.at<float>(1, 0))/outPoint3dHomo.at<float>(2, 0)+cy;
             //cout <<  outPoint3dHomo.at<float>(2, 0) <<  " " << outPoint3dHomo.at<float>(3, 0) <<endl;
             //cout << "u1 " << u1 << " v1 " <<  v1 << " u2 " << outKeypoint.pt.x << " v2 " << outKeypoint.pt.y <<endl;
 
             outPoints.push_back(outKeypoint);
 
         }
+
+        /*
+        Vec3d rvec(0,0,0); //Rodrigues(R ,rvec);
+        Vec3d tvec(0,0,0); // = P.col(3);
+        vector <Point2d> reprojected_pt_set1;
+        inPoint3dHomo2.convertTo(inPoint3dHomo2, CV_64FC1);
+        cameraMatrix.convertTo(cameraMatrix, CV_64FC1);
+        projectPoints(inPoint3dHomo2,rvec,tvec,cameraMatrix, Mat(),reprojected_pt_set1);
+        */
 
 
 
@@ -1189,7 +1201,7 @@ namespace vi
         cv::KeyPoint::convert(_current_frame->prevGoodMatches, points2_OK,point_indexs);
 
 
-
+        int lvl= 0;
         Mat E; // essential matrix
         double focal = fx;
         E = findEssentialMat(points1_OK, points2_OK,  focal, Point2d(cx, cy), RANSAC, 0.999, 1.0, noArray());
@@ -1200,8 +1212,8 @@ namespace vi
         float sy = TraslationResidual.at<float>(1,0);
         float sz = TraslationResidual.at<float>(2,0);
         Mat Rotation_ResCam = imu2camRotation.t()* imuCore.residual_rotationMatrix*imu2camRotation;
-        Rotation_ResCam.convertTo(Rotation_ResCam, CV_64FC1);
-        Mat trans_skew =  E*Rotation_ResCam.t();
+        //Rotation_ResCam.convertTo(Rotation_ResCam, CV_64FC1);
+        //Mat trans_skew =  E*Rotation_ResCam.t();
 
 
 
@@ -1214,14 +1226,50 @@ namespace vi
         //recoverPose(E, points1_OK, points1_OK, cameraMatrix, R_out, t_out, noArray());
         int p;
          p = recoverPose(E, points1_OK, points2_OK, R_out, t_out, focal, Point2d(cx, cy), noArray()   );
+                R_out.convertTo(R_out, CV_32FC1);
+                    t_out.convertTo(t_out, CV_32FC1);
+
+                         float scale = sqrt(sx*sx+sy*sy+sz*sz);
+
+
+        t_out = t_out*scale;
+
+
+        vector<KeyPoint> warpedDebugKeyPoints;
+        cout << "here"<<endl;
+        WarpFunctionRT(_current_frame->prevGoodMatches,  R_out, t_out, warpedDebugKeyPoints);
+        //WarpFunctionRT(_previous_frame->nextGoodMatches, Mat::eye(3, 3, CV_32FC1), trakity, warpedDebugKeyPoints);
+
+         // WarpFunctionRT(_current_frame->prevGoodMatches, Mat::eye(3, 3, CV_32FC1), Mat::zeros(3, 1, CV_32FC1), warpedDebugKeyPoints, 1.0);
+         //WarpFunctionRT(_current_frame->prevGoodMatches, Mat::eye(3, 3, CV_32FC1), Mat::zeros(3, 1, CV_32FC1), warpedDebugKeyPoints2, 10);
+
+        float u1 = warpedDebugKeyPoints[0].pt.x;
+        float v1 = warpedDebugKeyPoints[0].pt.y;
+        float u2 = _previous_frame->nextGoodMatches[0].pt.x;
+        float v2 = _previous_frame->nextGoodMatches[0].pt.y;
+        cout << "u1 " << u1 << " v1 " <<  v1 << " u2 " << u2<< " v2 " << v2 <<endl;
+
+        drawKeypoints(_previous_frame->grayImage[lvl], warpedDebugKeyPoints, currentImageDebugToShow, Scalar(0,255, 0), DrawMatchesFlags::DEFAULT);
+        drawKeypoints(currentImageDebugToShow, _previous_frame->nextGoodMatches, currentImageDebugToShow, Scalar(255,0, 0), DrawMatchesFlags::DEFAULT);
+        drawKeypoints(currentImageDebugToShow, _current_frame->prevGoodMatches, currentImageDebugToShow, Scalar(0,0, 255), DrawMatchesFlags::DEFAULT);
+        drawKeypoints(_current_frame->grayImage[lvl], _current_frame->prevGoodMatches, currentImageToShow, Scalar(255,0, 0), DrawMatchesFlags::DEFAULT);
+
+        imshow("currentDebugImage1", currentImageDebugToShow);
+        imshow("currentDebugImage2", currentImageToShow);
+        char c_input = (char) waitKey(-1);
+        if( c_input == 'q' | c_input == ((char)27) )  {
+                exit(0);
+        }
+
+
 
         Point3d residualRPYcam = rotationMatrix2RPY(R_out);//rotationMatrix2RPY(imu2camRotationRPY2rotationMatrix(imuCore.residualRPY));
         //cout << R_out<<endl;
         //cout << points1_OK.size()<< " " << points2_OK.size()<<endl;
         //cout << "ResidualRPY "<<residualRPYcam<<endl;
         //cout << "Residualtrans "<< t_out<<endl;
-            float scale = sqrt(sx*sx+sy*sy+sz*sz);
-
+
+        /*
 
         float sx1 = t_out.at<double>(0,0)*scale;
         float sy1 = t_out.at<double>(1,0)*scale;
@@ -1240,6 +1288,7 @@ namespace vi
         cout << "TransE1" << " sx1 " << sx1<< " sy1 " << sy1<< " sz1 " << sz1 <<endl;
 
         //cout << "TransE2" << " sx2 " << sx2<< " sy2 " << sy2<< " sz2 " << sz2 <<endl;
+        */
          /*
         Quaterniond residualQcam = toQuaternion(residualRPYcam.x, residualRPYcam.y, residualRPYcam.z) ;
         Quaternion quat_init(residualQcam.w, residualQcam.x, residualQcam.y, residualQcam.z); // w, x, y,

src/main_vi_slam.cpp

@@ -121,8 +121,10 @@ int main( int argc, char** argv ){
 
         qOrientationCamGT = toQuaternion(RPYOrientationCamGT.x, RPYOrientationCamGT.y, RPYOrientationCamGT.z);
         Mat transformationResidual = RPYAndPosition2transformationMatrix(RPYOrientationCamGTprev, positionCamGTprev).inv()*RPYAndPosition2transformationMatrix(RPYOrientationCamGT, positionCamGT);
+
         Mat Traslation_ResCamGT = Mat::ones(3, 1, CV_32FC1);  
         Mat Rotation_ResCamGt = transformationMatrix2rotationMatrix(transformationResidual);
+
         Traslation_ResCamGT.at<float>(0,0) = transformationResidual.at<float>(0,3);
         Traslation_ResCamGT.at<float>(1,0) = transformationResidual.at<float>(1,3);
         Traslation_ResCamGT.at<float>(2,0) = transformationResidual.at<float>(2,3);