added ondn similar to o1dn

src/rmcl/correction/MICP.cpp

@@ -193,11 +193,9 @@ bool MICP::loadSensor(
         sensor->data_topic.name = topic_name;
         std::cout << "    - topic:\t\t" << TC_TOPIC << sensor->data_topic.name << TC_END << std::endl;
 
-
         if(sensor_params->find("topic_type") != sensor_params->end())
         {
             sensor->data_topic.msg = sensor_params->at("topic_type")->data->as_string();
-            std::cout << "FOUND TOPIC TYPE: " << sensor->data_topic.msg << std::endl;
         }
 
 
@@ -361,9 +359,25 @@ bool MICP::loadSensor(
             model.range.min = model_params->at("range_min")->data->as_double();
             model.range.max = model_params->at("range_max")->data->as_double();
 
+            if(model_params->exists("width"))
+            {
+                // model.widt
+                model.width = model_params->at("width")->data->as_double();
+            } else {
+                // loading actual width from sensor data
+                model.width = 0;
+            }
+
+            if(model_params->exists("height"))
+            {
+                // model.widt
+                model.height = model_params->at("height")->data->as_double();
+            } else {
+                // loading actual height from sensor data
+                model.height = 0;
+            }
 
             std::vector<double> orig = {0.0, 0.0, 0.0};
-
             if(model_params->exists("orig"))
             {
               orig = model_params->at("orig")->data->as_double_array();
@@ -379,6 +393,30 @@ bool MICP::loadSensor(
             model.orig.y = orig[1];
             model.orig.z = orig[2];
 
+            if(model_params->exists("dirs"))
+            {
+                std::vector<double> dirs = model_params->at("dirs")->data->as_double_array();
+                model.dirs.resize(dirs.size() / 3);
+                for(size_t i=0; i<dirs.size() / 3; i++)
+                {
+                    model.dirs[i].x = dirs[i * 3 + 0];
+                    model.dirs[i].y = dirs[i * 3 + 1];
+                    model.dirs[i].z = dirs[i * 3 + 2];
+                }
+            } else {
+                model.dirs.resize(0);
+            }
+
+            sensor->model = model;
+            model_loaded = !model_loading_error;
+        } else if(sensor_type == "ondn") {
+            rm::OnDnModel model;
+
+            bool model_loading_error = false;
+
+            model.range.min = model_params->at("range_min")->data->as_double();
+            model.range.max = model_params->at("range_max")->data->as_double();
+
             if(model_params->exists("width"))
             {
                 // model.widt
@@ -397,6 +435,20 @@ bool MICP::loadSensor(
                 model.height = 0;
             }
 
+            if(model_params->exists("origs"))
+            {
+                std::vector<double> origs = model_params->at("origs")->data->as_double_array();
+                model.origs.resize(origs.size() / 3);
+                for(size_t i=0; i<origs.size() / 3; i++)
+                {
+                    model.origs[i].x = origs[i * 3 + 0];
+                    model.origs[i].y = origs[i * 3 + 1];
+                    model.origs[i].z = origs[i * 3 + 2];
+                }
+            } else {
+                model.origs.resize(0);
+            }
+
             if(model_params->exists("dirs"))
             {
                 std::vector<double> dirs = model_params->at("dirs")->data->as_double_array();
@@ -410,35 +462,6 @@ bool MICP::loadSensor(
             } else {
                 model.dirs.resize(0);
             }
-
-            sensor->model = model;
-            model_loaded = !model_loading_error;
-        } else if(sensor_type == "ondn") {
-            rm::OnDnModel model;
-
-            bool model_loading_error = false;
-
-            model.width  = model_params->at("width")->data->as_int();
-            model.height = model_params->at("height")->data->as_int();
-
-            model.range.min = model_params->at("range_min")->data->as_double();
-            model.range.max = model_params->at("range_max")->data->as_double();
-
-            std::vector<double> origs = model_params->at("origs")->data->as_double_array();
-            std::vector<double> dirs = model_params->at("dirs")->data->as_double_array();
-
-            model.origs.resize(origs.size() / 3);
-            model.dirs.resize(dirs.size() / 3);
-            for(size_t i=0; i<origs.size() / 3; i++)
-            {
-                model.origs[i].x = origs[i * 3 + 0];
-                model.origs[i].y = origs[i * 3 + 1];
-                model.origs[i].z = origs[i * 3 + 2];
-
-                model.dirs[i].x  = dirs[i * 3 + 0];
-                model.dirs[i].y  = dirs[i * 3 + 1];
-                model.dirs[i].z  = dirs[i * 3 + 2];
-            }
 
             sensor->model = model;
             model_loaded = !model_loading_error;
@@ -463,6 +486,11 @@ bool MICP::loadSensor(
 
         std::cout << "    - topic:\t\t" << TC_TOPIC << sensor->info_topic.name  << TC_END << std::endl;
 
+        if(sensor_params->find("model_topic_type") != sensor_params->end())
+        {
+            sensor->info_topic.msg = sensor_params->at("model_topic_type")->data->as_string();
+        }
+
         std::map<std::string, std::vector<std::string> > topic_map = m_nh->get_topic_names_and_types();
 
         if(topic_map.find(sensor->info_topic.name) != topic_map.end())
@@ -802,7 +830,6 @@ bool MICP::loadSensor(
     // connect to sensor topics
     sensor->connect();
 
-
 
     #ifdef RMCL_EMBREE
     if(sensor->type == 0) // spherical

src/rmcl/correction/MICPRangeSensor.cpp

@@ -541,8 +541,6 @@ void MICPRangeSensor::computeCovs(
 {
     // this is bad. maybe we must go away from having a completely generic sensor
 
-    // std::cout << "Compute Covs - CPU" << std::endl;
-
     #ifdef RMCL_EMBREE
     if(backend == 0)
     {