From 502e8a16b555434baea6c5d69f027fca0cab1a25 Mon Sep 17 00:00:00 2001
From: Carlos Guindel <cguindel@ing.uc3m.es>
Date: Mon, 25 Feb 2019 13:06:16 +0100
Subject: [PATCH] Fix minor code issues

---
 src/laser_pattern.cpp        | 27 +++++++++++++--------------
 src/levinson.cpp             | 25 +++++++------------------
 src/pcl_coloring.cpp         |  2 +-
 src/stereo_pattern.cpp       |  6 +++---
 src/velo2cam_calibration.cpp |  4 ++--
 5 files changed, 26 insertions(+), 38 deletions(-)

diff --git a/src/laser_pattern.cpp b/src/laser_pattern.cpp
index 2a99eb0..48650f4 100644
--- a/src/laser_pattern.cpp
+++ b/src/laser_pattern.cpp
@@ -132,22 +132,21 @@ void callback(const PointCloud2::ConstPtr& laser_cloud){
 
   // Get edges points by range
   vector<vector<Velodyne::Point*> > rings = Velodyne::getRings(*velocloud);
-  for (vector<vector<Velodyne::Point*> >::iterator ring = rings.begin(); ring < rings.end(); ring++){
-    Velodyne::Point *prev, *succ;
+  for (vector<vector<Velodyne::Point*> >::iterator ring = rings.begin(); ring < rings.end(); ++ring){
     if (ring->empty()) continue;
 
     (*ring->begin())->intensity = 0;
     (*(ring->end() - 1))->intensity = 0;
     for (vector<Velodyne::Point*>::iterator pt = ring->begin() + 1; pt < ring->end() - 1; pt++){
-      prev = *(pt - 1);
-      succ = *(pt + 1);
+      Velodyne::Point *prev = *(pt - 1);
+      Velodyne::Point *succ = *(pt + 1);
       (*pt)->intensity = max( max( prev->range-(*pt)->range, succ->range-(*pt)->range), 0.f);
     }
   }
 
   pcl::PointCloud<Velodyne::Point>::Ptr edges_cloud(new pcl::PointCloud<Velodyne::Point>);
   float THRESHOLD = 0.5 ;
-  for (pcl::PointCloud<Velodyne::Point>::iterator pt = velocloud->points.begin(); pt < velocloud->points.end(); pt++){
+  for (pcl::PointCloud<Velodyne::Point>::iterator pt = velocloud->points.begin(); pt < velocloud->points.end(); ++pt){
     if(pt->intensity>THRESHOLD){
       edges_cloud->push_back(*pt);
     }
@@ -169,7 +168,7 @@ void callback(const PointCloud2::ConstPtr& laser_cloud){
   pcl::PointCloud<pcl::PointXYZ>::Ptr circles_cloud(new pcl::PointCloud<pcl::PointXYZ>);
   vector<vector<Velodyne::Point*> > rings2 = Velodyne::getRings(*pattern_cloud);
   int ringsWithCircle = 0;
-  for (vector<vector<Velodyne::Point*> >::iterator ring = rings2.begin(); ring < rings2.end(); ring++){
+  for (vector<vector<Velodyne::Point*> >::iterator ring = rings2.begin(); ring < rings2.end(); ++ring){
     if(ring->size() < 4){
       ring->clear();
     }else{ // Remove first and last points in ring
@@ -177,7 +176,7 @@ void callback(const PointCloud2::ConstPtr& laser_cloud){
       ring->erase(ring->begin());
       ring->pop_back();
 
-      for (vector<Velodyne::Point*>::iterator pt = ring->begin(); pt < ring->end(); pt++){
+      for (vector<Velodyne::Point*>::iterator pt = ring->begin(); pt < ring->end(); ++pt){
         // Velodyne specific info no longer needed for calibration
         // so standard point is used from now on
         pcl::PointXYZ point;
@@ -245,9 +244,9 @@ void callback(const PointCloud2::ConstPtr& laser_cloud){
 
   //if(DEBUG) cout << cluster_indices.size() << " clusters found from " << xy_cloud->points.size() << " points in cloud" << endl;
 
-  for (std::vector<pcl::PointIndices>::iterator it=cluster_indices.begin(); it<cluster_indices.end(); it++) {
+  for (std::vector<pcl::PointIndices>::iterator it=cluster_indices.begin(); it<cluster_indices.end(); ++it) {
     float accx = 0., accy = 0., accz = 0.;
-    for(vector<int>::iterator it2=it->indices.begin(); it2<it->indices.end(); it2++){
+    for(vector<int>::iterator it2=it->indices.begin(); it2<it->indices.end(); ++it2){
       accx+=xy_cloud->at(*it2).x;
       accy+=xy_cloud->at(*it2).y;
       accz+=xy_cloud->at(*it2).z;
@@ -279,7 +278,7 @@ void callback(const PointCloud2::ConstPtr& laser_cloud){
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>); // Temp pc used for swaping
 
   // Force pattern points to belong to computed plane
-  for (pcl::PointCloud<pcl::PointXYZ>::iterator pt = copy_cloud->points.begin(); pt < copy_cloud->points.end(); pt++){
+  for (pcl::PointCloud<pcl::PointXYZ>::iterator pt = copy_cloud->points.begin(); pt < copy_cloud->points.end(); ++pt){
     pt->z = zcoord_xyplane;
   }
 
@@ -318,7 +317,7 @@ void callback(const PointCloud2::ConstPtr& laser_cloud){
     // if(DEBUG) ROS_INFO("Distance to centroid %f", centroid_distance);
     if (centroid_distance < centroid_distance_min_){
       valid = false;
-      for (pcl::PointCloud<pcl::PointXYZ>::iterator pt = circle_cloud->points.begin(); pt < circle_cloud->points.end(); pt++){
+      for (pcl::PointCloud<pcl::PointXYZ>::iterator pt = circle_cloud->points.begin(); pt < circle_cloud->points.end(); ++pt){
         centroid_cloud_inliers.push_back(*pt);
       }
     }else if(centroid_distance > centroid_distance_max_){
@@ -334,13 +333,13 @@ void callback(const PointCloud2::ConstPtr& laser_cloud){
       }
 
       // If center is valid, check if any point from wrong_circle belongs to it, and pop it if true
-      for (std::vector<pcl::PointXYZ>::iterator pt = centroid_cloud_inliers.begin(); pt < centroid_cloud_inliers.end(); pt++){
+      for (std::vector<pcl::PointXYZ>::iterator pt = centroid_cloud_inliers.begin(); pt < centroid_cloud_inliers.end(); ++pt){
         pcl::PointXYZ schrodinger_pt((*pt).x, (*pt).y, (*pt).z);
         double distance_to_cluster = sqrt(pow(schrodinger_pt.x-center.x,2) + pow(schrodinger_pt.y-center.y,2) + pow(schrodinger_pt.z-center.z,2));
         // if(DEBUG) ROS_INFO("Distance to cluster: %lf", distance_to_cluster);
         if(distance_to_cluster<circle_radius_+0.02){
           centroid_cloud_inliers.erase(pt);
-          pt--; // To avoid out of range
+          --pt; // To avoid out of range
         }
       }
       // if(DEBUG) ROS_INFO("Remaining inliers %lu", centroid_cloud_inliers.size());
@@ -366,7 +365,7 @@ void callback(const PointCloud2::ConstPtr& laser_cloud){
   }
 
   if(found_centers.size() >= min_centers_found_ && found_centers.size() < 5){
-    for (std::vector<std::vector<float> >::iterator it = found_centers.begin(); it < found_centers.end(); it++){
+    for (std::vector<std::vector<float> >::iterator it = found_centers.begin(); it < found_centers.end(); ++it){
       pcl::PointXYZ center;
       center.x = (*it)[0];
       center.y = (*it)[1];
diff --git a/src/levinson.cpp b/src/levinson.cpp
index af6b35a..195d908 100644
--- a/src/levinson.cpp
+++ b/src/levinson.cpp
@@ -112,15 +112,14 @@ COLOUR GetColour(double v,double vmin,double vmax)
 
 void edges_pointcloud(pcl::PointCloud<Velodyne::Point>::Ptr pc){
   std::vector<std::vector<Velodyne::Point*> > rings = Velodyne::getRings(*pc);
-  for (std::vector<std::vector<Velodyne::Point*> >::iterator ring = rings.begin(); ring < rings.end(); ring++){
-    Velodyne::Point *prev, *succ;
+  for (std::vector<std::vector<Velodyne::Point*> >::iterator ring = rings.begin(); ring < rings.end(); ++ring){
     if (ring->empty()) continue;
 
     (*ring->begin())->intensity = 0;
     (*(ring->end() - 1))->intensity = 0;
-    for (vector<Velodyne::Point*>::iterator pt = ring->begin() + 1; pt < ring->end() - 1; pt++){
-      prev = *(pt - 1);
-      succ = *(pt + 1);
+    for (vector<Velodyne::Point*>::iterator pt = ring->begin() + 1; pt < ring->end() - 1; ++pt){
+      Velodyne::Point *prev = *(pt - 1);
+      Velodyne::Point *succ = *(pt + 1);
       (*pt)->intensity = pow(std::max( std::max( prev->range-(*pt)->range, succ->range-(*pt)->range), 0.f), 0.5);
     }
   }
@@ -201,10 +200,9 @@ inline float calc(float &val, const float &psi, int row, int col, const cv::Mat&
 template <typename Type_in, typename Type_out>
 inline void
 neighbors_x_pos(cv::Mat &in, cv::Mat &out, float psi, float alpha){
-  float val = 0;
-
   for(int row = 0; row < in.rows; ++row)
   {
+    float val = 0;
     val = 0;
     for(int col = 0; col < in.cols; ++col)
     {
@@ -411,15 +409,6 @@ void checkExtrinics(const sensor_msgs::CameraInfoConstPtr& cinfo_msg){
         for (int iter_r=0; iter_r<MAX; iter_r++){
           for (int iter_p=0; iter_p<MAX; iter_p++){
             for (int iter_yaw=0; iter_yaw<MAX; iter_yaw++){
-
-              x_mod = x;
-              y_mod = y;
-              z_mod = z;
-
-              r_mod = r;
-              p_mod = p;
-              yaw_mod = yaw;
-
               x_mod = x + (iter_x-M)*INC;
               y_mod = y + (iter_y-M)*INC;
               z_mod = z + (iter_z-M)*INC;
@@ -444,7 +433,7 @@ void checkExtrinics(const sensor_msgs::CameraInfoConstPtr& cinfo_msg){
 
               double objective_function = 0;
 
-              for (pcl::PointCloud<Velodyne::Point>::iterator pt = trans_cloud->points.begin(); pt < trans_cloud->points.end(); pt++)
+              for (pcl::PointCloud<Velodyne::Point>::iterator pt = trans_cloud->points.begin(); pt < trans_cloud->points.end(); ++pt)
               {
                 cv::Point3d pt_cv((*pt).x, (*pt).y, (*pt).z);
                 cv::Point2d uv;
@@ -494,7 +483,7 @@ void laser_callback(const sensor_msgs::PointCloud2::ConstPtr& velo_cloud){
   edges_cloud =  pcl::PointCloud<Velodyne::Point>::Ptr(new pcl::PointCloud<Velodyne::Point>);
 
   float THRESHOLD = 0.30;
-  for (pcl::PointCloud<Velodyne::Point>::iterator pt = laser_cloud->points.begin(); pt < laser_cloud->points.end(); pt++){
+  for (pcl::PointCloud<Velodyne::Point>::iterator pt = laser_cloud->points.begin(); pt < laser_cloud->points.end(); ++pt){
     if(pow(pt->intensity,2)>THRESHOLD){
       edges_cloud->push_back(*pt);
     }
diff --git a/src/pcl_coloring.cpp b/src/pcl_coloring.cpp
index 3237290..07f36e8 100644
--- a/src/pcl_coloring.cpp
+++ b/src/pcl_coloring.cpp
@@ -83,7 +83,7 @@ void callback(const PointCloud2::ConstPtr& pcl_msg, const CameraInfoConstPtr& ci
 
   pcl::copyPointCloud(*trans_cloud, *coloured);
 
-  for (pcl::PointCloud<pcl::PointXYZRGB>::iterator pt = coloured->points.begin(); pt < coloured->points.end(); pt++)
+  for (pcl::PointCloud<pcl::PointXYZRGB>::iterator pt = coloured->points.begin(); pt < coloured->points.end(); ++pt)
   {
     cv::Point3d pt_cv((*pt).x, (*pt).y, (*pt).z);
     cv::Point2d uv;
diff --git a/src/stereo_pattern.cpp b/src/stereo_pattern.cpp
index 5715f91..6bf936a 100644
--- a/src/stereo_pattern.cpp
+++ b/src/stereo_pattern.cpp
@@ -205,7 +205,7 @@ void callback(const PointCloud2::ConstPtr& camera_cloud,
   pcl::PointCloud<pcl::PointXYZ>::Ptr cam_plane_wol_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
 
   // Remove them
-  for (vector<pcl::ModelCoefficients>::iterator it=out_lines.begin(); it<out_lines.end(); it++){
+  for (vector<pcl::ModelCoefficients>::iterator it=out_lines.begin(); it<out_lines.end(); ++it){
 
     pcl::PointIndices::Ptr line_ind (new pcl::PointIndices);
     pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr dil (new pcl::SampleConsensusModelLine<pcl::PointXYZ> (cam_plane_cloud));
@@ -270,7 +270,7 @@ void callback(const PointCloud2::ConstPtr& camera_cloud,
 
   // Force pattern points to belong to computed plane
   for (pcl::PointCloud<pcl::PointXYZ>::iterator pt = xy_cloud->points.begin();
-  pt < xy_cloud->points.end(); pt++){
+  pt < xy_cloud->points.end(); ++pt){
     pt->z = zcoord_xyplane;
   }
 
@@ -348,7 +348,7 @@ void callback(const PointCloud2::ConstPtr& camera_cloud,
     ROS_WARN("Not enough centers: %ld", found_centers.size());
     return;
   }else{
-    for (std::vector<std::vector<float> >::iterator it = found_centers.begin(); it < found_centers.end(); it++){
+    for (std::vector<std::vector<float> >::iterator it = found_centers.begin(); it < found_centers.end(); ++it){
       pcl::PointXYZ center;
       center.x = (*it)[0];
       center.y = (*it)[1];
diff --git a/src/velo2cam_calibration.cpp b/src/velo2cam_calibration.cpp
index cfe891c..a239c7c 100644
--- a/src/velo2cam_calibration.cpp
+++ b/src/velo2cam_calibration.cpp
@@ -306,7 +306,7 @@ void laser_callback(const velo2cam_calibration::ClusterCentroids::ConstPtr velo_
 
   if(DEBUG) ROS_INFO("[V2C] LASER");
 
-  for(vector<pcl::PointXYZ>::iterator it=lv.begin(); it<lv.end(); it++){
+  for(vector<pcl::PointXYZ>::iterator it=lv.begin(); it<lv.end(); ++it){
     if (DEBUG) cout << "l" << it - lv.begin() << "="<< "[" << (*it).x << " " << (*it).y << " " << (*it).z << "]" << endl;
   }
 
@@ -391,7 +391,7 @@ void stereo_callback(velo2cam_calibration::ClusterCentroids::ConstPtr image_cent
 
   if(DEBUG) ROS_INFO("[V2C] CAMERA");
 
-  for(vector<pcl::PointXYZ>::iterator it=cv.begin(); it<cv.end(); it++){
+  for(vector<pcl::PointXYZ>::iterator it=cv.begin(); it<cv.end(); ++it){
     if (DEBUG) cout << "c" << it - cv.begin() << "="<< "[" << (*it).x << " " << (*it).y << " " << (*it).z << "]"<<endl;
   }