some modifications

Author: shaozu

vins_estimator/CMakeLists.txt

@@ -17,7 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
 
 find_package(OpenCV REQUIRED)
 
-message(WARNING "OpenCV_VERSION: ${OpenCV_VERSION}")
+# message(WARNING "OpenCV_VERSION: ${OpenCV_VERSION}")
 
 find_package(Ceres REQUIRED)
 

vins_estimator/src/estimator.cpp

@@ -33,9 +33,7 @@ void Estimator::clearState()
         angular_velocity_buf[i].clear();
 
         if (pre_integrations[i] != nullptr)
-        {
             delete pre_integrations[i];
-        }
         pre_integrations[i] = nullptr;
     }
 
@@ -45,6 +43,15 @@ void Estimator::clearState()
         ric[i] = Matrix3d::Identity();
     }
 
+    for (auto &it : all_image_frame)
+    {
+        if (it.second.pre_integration != nullptr)
+        {
+            delete it.second.pre_integration;
+            it.second.pre_integration = nullptr;
+        }
+    }
+
     solver_flag = INITIAL;
     first_imu = false,
     sum_of_back = 0;
@@ -1000,6 +1007,7 @@ void Estimator::slideWindow()
     TicToc t_margin;
     if (marginalization_flag == MARGIN_OLD)
     {
+        double t_0 = Headers[0].stamp.toSec();
         back_R0 = Rs[0];
         back_P0 = Ps[0];
         if (frame_count == WINDOW_SIZE)
@@ -1036,11 +1044,20 @@ void Estimator::slideWindow()
 
             if (true || solver_flag == INITIAL)
             {
-                double t_0 = Headers[0].stamp.toSec();
                 map<double, ImageFrame>::iterator it_0;
                 it_0 = all_image_frame.find(t_0);
                 delete it_0->second.pre_integration;
+                it_0->second.pre_integration = nullptr;
+ 
+                for (map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it_0; ++it)
+                {
+                    if (it->second.pre_integration)
+                        delete it->second.pre_integration;
+                    it->second.pre_integration = NULL;
+                }
+
                 all_image_frame.erase(all_image_frame.begin(), it_0);
+                all_image_frame.erase(t_0);
 
             }
             slideWindowOld();

vins_estimator/src/factor/marginalization_factor.cpp

@@ -73,7 +73,7 @@ MarginalizationInfo::~MarginalizationInfo()
     //ROS_WARN("release marginlizationinfo");
 
     for (auto it = parameter_block_data.begin(); it != parameter_block_data.end(); ++it)
-        delete it->second;
+        delete[] it->second;
 
     for (int i = 0; i < (int)factors.size(); i++)
     {