Removing the std::vector length error

CMakeLists.txt

@@ -10,7 +10,7 @@ add_definitions(${PCL_DEFINITIONS})
 
 include_directories(${PCL_INCLUDE_DIRS} include)
 
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
 
 FILE(GLOB SRCS0 src/mapping/*.cpp)
 FILE(GLOB SRCS1 src/calibration/*.cpp)
@@ -22,5 +22,5 @@ target_link_libraries (mapping ${PCL_LIBRARIES} ${LIBS})
 add_executable (calibration  ${SRCS1} )
 target_link_libraries (calibration ${PCL_LIBRARIES} ${LIBS})
 
-add_executable (fitline  ${SRCS2} )
-target_link_libraries (fitline ${PCL_LIBRARIES} ${LIBS})
+add_executable (ransac  ${SRCS2} )
+target_link_libraries (ransac ${PCL_LIBRARIES} ${LIBS})

README.md

@@ -55,7 +55,7 @@ This operation will generate three files, namely **mapping**, **calibration**, *
 
 ### **2. run**
 ```
-cd livox_calibration
+cd Livox_automatic_calibration
 cp run.sh build/
 cd build
 sh run.sh

include/ransac.h

@@ -1,12 +1,23 @@
 #ifndef _RANSAC_H_
 #define _RANSAC_H_
 #include<cstdlib>
-#include"fitline.h"
+// #include"fitline.h"
+// #include"type.h"
+#include<cmath>
 #include<iostream>
 #include<ctime>
 #include<cstring>
 #include<cstdio>
+#include <vector>
+// #include <cstdio>
 
-float Ransac(Point2D32f* points, size_t Cnt, float *lines);
-float Ransac(Point2D32f* points, size_t Cnt, float *line, int numForEstimate, float successProbability, float maxOutliersPercentage);
+
+typedef struct
+{
+	float x;
+	float y;
+}Point2D32f;
+
+// float Ransac(Point2D32f* points, size_t Cnt, float *lines);
+float Ransac(std::vector<Point2D32f>& points, size_t Cnt, float *line, int numForEstimate, float successProbability, float maxOutliersPercentage);
 #endif

src/calibration/calibration.cpp

@@ -56,13 +56,25 @@ POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/filters/approximate_voxel_grid.h>
 #include <pcl/registration/gicp.h>
 
+#include <iostream>
+#include <ctime>
+#include <chrono>
+
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
 #include <geometry_utils/Transform3.h>
 #include <point_cloud_mapper/PointCloudMapper.h>
 
 using namespace std;
+
+using Time = std::chrono::steady_clock;
+using ms = std::chrono::milliseconds;
+using float_sec = std::chrono::duration<float>;
+using float_time_point = std::chrono::time_point<Time, float_sec>;
+
+float_time_point getCurrentTime() { return Time::now(); }
+
 namespace gu = geometry_utils;
 
 #define PI (3.1415926535897932346f)
@@ -148,15 +160,14 @@ int main()
 
     //=================================
     //prepare display
-    boost::shared_ptr<pcl::visualization::PCLVisualizer>
-        viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
-    viewer_final->setBackgroundColor(0, 0, 0);
-    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> map_color(H_LiDAR_Map, 255, 0, 0);
-    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> match_color(H_LiDAR_Map, 0, 255, 0);
+    //boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
+    //viewer_final->setBackgroundColor(0, 0, 0);
+    //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> map_color(H_LiDAR_Map, 255, 0, 0);
+    //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> match_color(H_LiDAR_Map, 0, 255, 0);
 
-    viewer_final->addPointCloud<pcl::PointXYZ>(H_LiDAR_Map, map_color, "target cloud");
-    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
-    viewer_final->addPointCloud<pcl::PointXYZ>(H_LiDAR_Map, match_color, "match cloud"); //display the match cloud
+    //viewer_final->addPointCloud<pcl::PointXYZ>(H_LiDAR_Map, map_color, "target cloud");
+    //viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
+    //viewer_final->addPointCloud<pcl::PointXYZ>(H_LiDAR_Map, match_color, "match cloud"); //display the match cloud
 
     //=================================
     // prepare save matrix
@@ -169,9 +180,20 @@ int main()
     //=================================
     //              START
     //=================================
-    while (!viewer_final->wasStopped())
+    pcl::PointCloud<pcl::PointXYZ>::Ptr frames(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr trans_output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr final_output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr neighbors_trans(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr neighbors_L(new pcl::PointCloud<pcl::PointXYZ>); //201 neighbors points from nap202
+
+    cout<< "Start the long iterations...\n";
+
+    //while (!viewer_final->wasStopped())
+    while(1)
     {
-        pcl::PointCloud<pcl::PointXYZ>::Ptr frames(new pcl::PointCloud<pcl::PointXYZ>);
+        float_time_point startTime000 = getCurrentTime();
+    	frames->clear();
+        //pcl::PointCloud<pcl::PointXYZ>::Ptr frames(new pcl::PointCloud<pcl::PointXYZ>);
         if (pcl::io::loadPCDFile<pcl::PointXYZ>(string(framesDir) + "/" + itos(frame_count) + ".pcd", *frames) == -1)
         {
             PCL_ERROR("Couldn't read H_LiDAR_Map \n");
@@ -190,14 +212,26 @@ int main()
 
         //================== Step.4 Start calibration =====================//
 
-        pcl::PointCloud<pcl::PointXYZ>::Ptr trans_output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-        pcl::PointCloud<pcl::PointXYZ>::Ptr final_output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+        //pcl::PointCloud<pcl::PointXYZ>::Ptr trans_output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+        //pcl::PointCloud<pcl::PointXYZ>::Ptr final_output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+        trans_output_cloud->clear();
+        final_output_cloud->clear();
+        float_time_point Time001 = getCurrentTime();
+
         pcl::transformPointCloud(*frames, *trans_output_cloud, init_guess); //Tiny_T * init_guess   ->update this matrix
         pcl::transformPointCloud(*trans_output_cloud, *final_output_cloud, T_Matrix);
 
-        pcl::PointCloud<pcl::PointXYZ>::Ptr neighbors_L(new pcl::PointCloud<pcl::PointXYZ>); //201 neighbors points from nap202
+        float_time_point Time002 = getCurrentTime();
+
+        //pcl::PointCloud<pcl::PointXYZ>::Ptr neighbors_L(new pcl::PointCloud<pcl::PointXYZ>); //201 neighbors points from nap202
+        neighbors_L->clear();
+
         maps.ApproxNearestNeighbors(*final_output_cloud, neighbors_L.get());
 
+        float_time_point Time003 = getCurrentTime();
+
+        // cout << "Elapsed_nn(ms)=" << since(nn_start).count() << "\n";
+
         //INVERSE T_mat==============================
 
         gu::Transform3 inverse_mat;
@@ -215,19 +249,33 @@ int main()
         T_Matrix_Inverse.block(0, 3, 3, 1) = T_Matrix_Inverse_T;
 
         //====== Core step ======//
+        float_time_point Time004 = getCurrentTime();
 
-        pcl::PointCloud<pcl::PointXYZ>::Ptr neighbors_trans(new pcl::PointCloud<pcl::PointXYZ>);
+        //pcl::PointCloud<pcl::PointXYZ>::Ptr neighbors_trans(new pcl::PointCloud<pcl::PointXYZ>);
+        neighbors_trans->clear();
         pcl::transformPointCloud(*neighbors_L, *neighbors_trans, T_Matrix_Inverse);
 
+        float_time_point Time005 = getCurrentTime();
+
         //Do ICP and get the tiny trans T
         icp.setInputSource(trans_output_cloud); //201
+        float_time_point Time0051 = getCurrentTime();
         icp.setInputTarget(neighbors_trans);    //202 (201's neighbor's point cloud)
+        float_time_point Time0052 = getCurrentTime();
         icp.align(*ICP_output_cloud);
+        float_time_point Time0053 = getCurrentTime();
         const Eigen::Matrix4f Tiny_T = icp.getFinalTransformation();
 
-        //std::cout << "Score: " << icp.getFitnessScore() << std::endl;
+        std::cout << "Score: " << icp.getFitnessScore() << std::endl;
+        float_time_point Time006 = getCurrentTime();
 
-        if (icp.getFitnessScore() > 1)
+        // std::cout << "Long times: " << ( Time0051 - Time005 ).count()*1000 << " "
+        //                             << ( Time0052 - Time0051 ).count()*1000 << " "
+        //                             << ( Time0053 - Time0052 ).count()*1000 << " "
+        //                             << ( Time006 - Time0053 ).count()*1000 << "\n";
+
+        if (icp.getFitnessScore() > 3 )
+        // if (icp.getFitnessScore() > 10 )
         {
             //std::cout<<"not match, skip this"<<std::endl;
             init_guess = init_guess_0;
@@ -250,16 +298,35 @@ int main()
             const Eigen::Matrix<double, 3, 1> EulerAngle_T = EulerAngle.Eigen();
             //std::cout<<"EulerAngle:  "<<EulerAngle_T(0,0)<<"  "<<EulerAngle_T(1,0)<<"  "<<EulerAngle_T(2,0)<<"  "<<std::endl;
 
-            if (icp.getFitnessScore() < 0.1)
+            // if (icp.getFitnessScore() < 0.1)
+            if (icp.getFitnessScore() < 1)
                 fout << frame_count - 100000 << " " << icp.getFitnessScore() << " " << Final_Calib_T(0, 3) << " " << Final_Calib_T(1, 3) << " " << Final_Calib_T(2, 3) << " " << EulerAngle_T(0, 0) << " " << EulerAngle_T(1, 0) << " " << EulerAngle_T(2, 0) << endl; //x,y,z,roll,pitch,yaw
+
+            cout << frame_count - 100000 << " " << icp.getFitnessScore() << " " << Final_Calib_T(0, 3) << " " << Final_Calib_T(1, 3) << " " << Final_Calib_T(2, 3) << " " << EulerAngle_T(0, 0) << " " << EulerAngle_T(1, 0) << " " << EulerAngle_T(2, 0) << endl; //x,y,z,roll,pitch,yaw
         }
 
         frame_count++;
         cframe_count++;
 
         printProgress((double)cframe_count / (double)framenumbers);
-        viewer_final->updatePointCloud<pcl::PointXYZ>(final_output_cloud, match_color, "match cloud");
-        viewer_final->spinOnce(10);
+
+        //viewer_final->updatePointCloud<pcl::PointXYZ>(final_output_cloud, match_color, "match cloud");
+        //viewer_final->removePointCloud("match cloud");
+        //viewer_final->addPointCloud<pcl::PointXYZ>(final_output_cloud, match_color, "match cloud");
+
+        //viewer_final->spinOnce(10);
+
+        // cout << "Elapsed(ms)=" << since(start).count() << "\n";
+        float_time_point Time007 = getCurrentTime();
+
+        std::cout <<"Progress: "<< 100 * (double)cframe_count / (double)framenumbers
+                <<" %   Timea1234567: " << ( Time007 - startTime000 ).count()*1000 << " " 
+                                        << ( Time002 - Time001 ).count()*1000 <<" "                                      
+                                        << ( Time003 - Time002 ).count()*1000 <<" "  
+                                        << ( Time004 - Time003 ).count()*1000 <<" "  
+                                        << ( Time005 - Time004 ).count()*1000 <<" "  
+                                        << ( Time006 - Time005 ).count()*1000 <<" "
+                                        << ( Time007 - Time006 ).count()*1000 <<"\n";
 
         if (cframe_count == framenumbers)
         {

src/ransac/main.cpp

@@ -1,4 +1,4 @@
-#include "fitline.h"
+// #include "fitline.h"
 #include "ransac.h"
 #include <cstdlib>
 #include <cstdio>
@@ -24,6 +24,7 @@ int main()
 
         ifstream calib_FileA("../data/calib_data.txt");
         int filecount = 0;
+
         while (!calib_FileA.eof())
         {
                 calib_FileA >> id >> score >> x >> y >> z >> roll >> pitch >> yaw;
@@ -36,7 +37,11 @@ int main()
         ifstream calib_File("../data/calib_data.txt");
         int count = 0;
 
-        Point2D32f points_x[filecount];
+        // Point2D32f points_x[filecount];
+        vector<Point2D32f> points_x;
+        Point2D32f tmp; tmp.x=0;tmp.y=0;
+        for(int jj=0;jj<filecount ; jj++) points_x.push_back(tmp);
+
         Point2D32f points_y[filecount];
         Point2D32f points_z[filecount];
 
@@ -82,7 +87,13 @@ int main()
         pitch_0=pitch_0/filecount;
         yaw_0=yaw_0/filecount;
 
-     /*   float lines[4] = {0.0}; //line parameters
+///------------------------------------------
+
+        float* lines = new float[4];
+        lines[0] = 0.000000000001;
+        lines[1] = 0;
+        lines[2] = 0;
+        lines[3] = 0;
 
         int numForEstimate = 5;
         float successProbability = 0.9999f;
@@ -95,36 +106,38 @@ int main()
         printf("x ransac fit(including outliers): a: %f  x: %f\n", a, b);
         x = b;
 
-        Ransac(points_y, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
-        a = lines[1] / lines[0];
-        b = lines[3] - a * lines[2];
-        printf("y ransac fit(including outliers): a: %f  y: %f\n", a, b);
-        y = b;
-
-        Ransac(points_z, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
-        a = lines[1] / lines[0];
-        b = lines[3] - a * lines[2];
-        printf("z ransac fit(including outliers): a: %f  z: %f\n", a, b);
-        z = b;
-
-        Ransac(points_roll, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
-        a = lines[1] / lines[0];
-        b = lines[3] - a * lines[2];
-        printf("roll ransac fit(including outliers): a: %f  roll: %f\n", a, b);
-        roll = b;
-
-        Ransac(points_pitch, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
-        a = lines[1] / lines[0];
-        b = lines[3] - a * lines[2];
-        printf("pitch ransac fit(including outliers): a: %f  pitch: %f\n", a, b);
-        pitch = b;
+        // Ransac(points_y, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
+        // a = lines[1] / lines[0];
+        // b = lines[3] - a * lines[2];
+        // printf("y ransac fit(including outliers): a: %f  y: %f\n", a, b);
+        // y = b;
+
+        // Ransac(points_z, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
+        // a = lines[1] / lines[0];
+        // b = lines[3] - a * lines[2];
+        // printf("z ransac fit(including outliers): a: %f  z: %f\n", a, b);
+        // z = b;
+
+        // Ransac(points_roll, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
+        // a = lines[1] / lines[0];
+        // b = lines[3] - a * lines[2];
+        // printf("roll ransac fit(including outliers): a: %f  roll: %f\n", a, b);
+        // roll = b;
+
+        // Ransac(points_pitch, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
+        // a = lines[1] / lines[0];
+        // b = lines[3] - a * lines[2];
+        // printf("pitch ransac fit(including outliers): a: %f  pitch: %f\n", a, b);
+        // pitch = b;
+
+        // Ransac(points_yaw, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
+        // a = lines[1] / lines[0];
+        // b = lines[3] - a * lines[2];
+        // printf("yaw ransac fit(including outliers): a: %f  yaw: %f\n", a, b);
+        // yaw = b;
+
+///------------------------------------------
 
-        Ransac(points_yaw, filecount, lines, numForEstimate, successProbability, maxOutliersPercentage);
-        a = lines[1] / lines[0];
-        b = lines[3] - a * lines[2];
-        printf("yaw ransac fit(including outliers): a: %f  yaw: %f\n", a, b);
-        yaw = b;
-*/
         calib_File.close();
 
         cout << endl;