Cleaned up extra params left over from GP

keevindoherty · keevindoherty · commit 37dfde308936 · 2017-02-26T18:40:52.000-05:00
diff --git a/config/methods/bgkoctomap.yaml b/config/methods/bgkoctomap.yaml
@@ -9,15 +9,6 @@ block_depth: 3 # Test-data octree depth (see Wang & Englot ICRA 2016)
 sf2: 10.0 # Actually sigma_0 in sparse kernel
 ell: 0.3 # Length scale of the sparse kernel
 
-# Old params from GP
-noise: 0.01 # From GP
-l: 100 # From GP
-
-# Variances
-max_var: 1000
-min_var: 0.001
-max_known_var: 0.02
-
 # Sampling resolutions
 free_resolution: 0.5 # Free space sampling resolution
 ds_resolution: 0.1 # Downsampling factor
@@ -29,4 +20,4 @@ var_thresh: 1000.0 # Threshold on variance to distinguish known/unknown
 
 # BGK Inference positive and negative class prior pseudocounts
 prior_A: 0.001 # Positive class (occupied)
-prior_B: 0.001 # Negative class (unoccupied)
+prior_B: 0.001 # Negative class (unoccupied)
diff --git a/config/methods/octomap.yaml b/config/methods/octomap.yaml
@@ -5,19 +5,6 @@ topic: /occupied_cells_vis_array
 resolution: 0.1
 block_depth: 3 # Test-data octree depth (see Wang & Englot ICRA 2016)
 
-# Kernel parameters
-sf2: 10.0 # Actually sigma_0 in sparse kernel
-ell: 0.3 # Length scale of the sparse kernel
-
-# Old params from GP
-noise: 0.01 # From GP
-l: 100 # From GP
-
-# Variances
-max_var: 1000
-min_var: 0.001
-max_known_var: 0.02
-
 # Sampling resolutions
 free_resolution: 0.5 # Free space sampling resolution
 ds_resolution: 0.1 # Downsampling factor
@@ -26,7 +13,3 @@ ds_resolution: 0.1 # Downsampling factor
 free_thresh: free_thresh
 occupied_thresh: 0.7
 var_thresh: 1000.0 # Threshold on variance to distinguish known/unknown
-
-# BGK Inference positive and negative class prior pseudocounts
-prior_A: 0.001 # Positive class (occupied)
-prior_B: 0.001 # Negative class (unoccupied)
diff --git a/include/bgkoctomap.h b/include/bgkoctomap.h
@@ -47,12 +47,15 @@ namespace la3dm {
          * @param free_thresh free threshold for Occupancy probability (default 0.3)
          * @param occupied_thresh occupied threshold for Occupancy probability (default 0.7)
          */
-        BGKOctoMap(float resolution, unsigned short block_depth, float sf2, float ell, float noise, float l,
-                  float min_var,
-                  float max_var, float max_known_var, float free_thresh, float occupied_thresh,
-                  float var_thresh,
-                  float prior_A,
-                  float prior_B);
+        BGKOctoMap(float resolution,
+                unsigned short block_depth,
+                float sf2,
+                float ell,
+                float free_thresh,
+                float occupied_thresh,
+                float var_thresh,
+                float prior_A,
+                float prior_B);
 
         ~BGKOctoMap();
 
diff --git a/src/bgkoctomap.cpp b/src/bgkoctomap.cpp
@@ -17,14 +17,26 @@ std::cout << "Debug: " << msg << std::endl; }
 
 namespace la3dm {
 
-    BGKOctoMap::BGKOctoMap() : BGKOctoMap(0.1f, 4, 1.0, 1.0, 0.01, 100, 0.001f, 1000.0f, 0.02f, 0.3f, 0.7f, 1.0f, 1.0f, 1.0f) { }
-
-    BGKOctoMap::BGKOctoMap(float resolution, unsigned short block_depth, float sf2, float ell, float noise, float l,
-                         float min_var,
-                         float max_var, float max_known_var, float free_thresh, float occupied_thresh,
-                         float var_thresh,
-                         float prior_A,
-                         float prior_B)
+    BGKOctoMap::BGKOctoMap() : BGKOctoMap(0.1f, // resolution
+                                        4, // block_depth
+                                        1.0, // sf2
+                                        1.0, // ell
+                                        0.3f, // free_thresh
+                                        0.7f, // occupied_thresh
+                                        1.0f, // var_thresh
+                                        1.0f, // prior_A
+                                        1.0f // prior_B
+                                    ) { }
+
+    BGKOctoMap::BGKOctoMap(float resolution,
+                        unsigned short block_depth,
+                        float sf2,
+                        float ell,
+                        float free_thresh,
+                        float occupied_thresh,
+                        float var_thresh,
+                        float prior_A,
+                        float prior_B)
             : resolution(resolution), block_depth(block_depth),
               block_size((float) pow(2, block_depth - 1) * resolution) {
         Block::resolution = resolution;
diff --git a/src/bgkoctomap_static_node.cpp b/src/bgkoctomap_static_node.cpp
@@ -28,11 +28,6 @@ int main(int argc, char **argv) {
     int block_depth = 4;
     double sf2 = 1.0;
     double ell = 1.0;
-    double noise = 0.01;
-    double l = 100;
-    double min_var = 0.001;
-    double max_var = 1000;
-    double max_known_var = 0.02;
     double free_resolution = 0.5;
     double ds_resolution = 0.1;
     double free_thresh = 0.3;
@@ -53,11 +48,6 @@ int main(int argc, char **argv) {
     nh.param<int>("block_depth", block_depth, block_depth);
     nh.param<double>("sf2", sf2, sf2);
     nh.param<double>("ell", ell, ell);
-    nh.param<double>("noise", noise, noise);
-    nh.param<double>("l", l, l);
-    nh.param<double>("min_var", min_var, min_var);
-    nh.param<double>("max_var", max_var, max_var);
-    nh.param<double>("max_known_var", max_known_var, max_known_var);
     nh.param<double>("free_resolution", free_resolution, free_resolution);
     nh.param<double>("ds_resolution", ds_resolution, ds_resolution);
     nh.param<double>("free_thresh", free_thresh, free_thresh);
@@ -79,10 +69,6 @@ int main(int argc, char **argv) {
             "block_depth: " << block_depth << std::endl <<
             "sf2: " << sf2 << std::endl <<
             "ell: " << ell << std::endl <<
-            "l: " << l << std::endl <<
-            "min_var: " << min_var << std::endl <<
-            "max_var: " << max_var << std::endl <<
-            "max_known_var: " << max_known_var << std::endl <<
             "free_resolution: " << free_resolution << std::endl <<
             "ds_resolution: " << ds_resolution << std::endl <<
             "free_thresh: " << free_thresh << std::endl <<
@@ -95,7 +81,7 @@ int main(int argc, char **argv) {
             "prior_B: " << prior_B
             );
 
-    la3dm::BGKOctoMap map(resolution, block_depth, sf2, ell, noise, l, min_var, max_var, max_known_var, free_thresh, occupied_thresh, var_thresh, prior_A, prior_B);
+    la3dm::BGKOctoMap map(resolution, block_depth, sf2, ell, free_thresh, occupied_thresh, var_thresh, prior_A, prior_B);
 
     ros::Time start = ros::Time::now();
     for (int scan_id = 1; scan_id <= scan_num; ++scan_id) {
diff --git a/src/octomap_static_node.cpp b/src/octomap_static_node.cpp
@@ -74,33 +74,33 @@ int main(int argc, char **argv) {
     ROS_INFO_STREAM("Mapping finished in " << (end - start).toSec() << "s");
 
     ///////// Compute Frontiers /////////////////////
-    ROS_INFO_STREAM("Computing frontiers");
-    la3dm::MarkerArrayPub f_pub(nh, "frontier_map", resolution);
-    for (auto it = oc.begin_leafs(); it != oc.end_leafs(); ++it) {
-        if (oc.isNodeOccupied(*it))
-            continue;
-
-        if (it.getZ() > 1.0 || it.getZ() < 0.3)
-            continue;
-
-        octomap::OcTreeKey key = it.getKey();
-        octomap::OcTreeKey nkey;
-        int n_unknown = 0;
-        for (nkey[2] = key[2] - 1; nkey[2] <= key[2] + 1; ++nkey[2]) {
-            for (nkey[1] = key[1] - 1; nkey[1] <= key[1] + 1; ++nkey[1]){
-                for (nkey[0] = key[0] - 1; nkey[0] <= key[0] + 1; ++nkey[0]){
-                    if (key != nkey){
-                        octomap::OcTreeNode* node = oc.search(nkey);
-                        n_unknown += node == NULL;
-                    }
-                }
-            }
-        }
-        if (n_unknown >= 4) {
-            f_pub.insert_point3d(it.getX(), it.getY(), it.getZ());
-        }
-    }
-    f_pub.publish();
+    // ROS_INFO_STREAM("Computing frontiers");
+    // la3dm::MarkerArrayPub f_pub(nh, "frontier_map", resolution);
+    // for (auto it = oc.begin_leafs(); it != oc.end_leafs(); ++it) {
+    //     if (oc.isNodeOccupied(*it))
+    //         continue;
+
+    //     if (it.getZ() > 1.0 || it.getZ() < 0.3)
+    //         continue;
+
+    //     octomap::OcTreeKey key = it.getKey();
+    //     octomap::OcTreeKey nkey;
+    //     int n_unknown = 0;
+    //     for (nkey[2] = key[2] - 1; nkey[2] <= key[2] + 1; ++nkey[2]) {
+    //         for (nkey[1] = key[1] - 1; nkey[1] <= key[1] + 1; ++nkey[1]){
+    //             for (nkey[0] = key[0] - 1; nkey[0] <= key[0] + 1; ++nkey[0]){
+    //                 if (key != nkey){
+    //                     octomap::OcTreeNode* node = oc.search(nkey);
+    //                     n_unknown += node == NULL;
+    //                 }
+    //             }
+    //         }
+    //     }
+    //     if (n_unknown >= 4) {
+    //         f_pub.insert_point3d(it.getX(), it.getY(), it.getZ());
+    //     }
+    // }
+    // f_pub.publish();
 
     ///////// Publish Map /////////////////////
     la3dm::MarkerArrayPub m_pub(nh, map_topic, resolution);
