Use YARP logging in sceneReconstruction app

PeterBowman · PeterBowman · commit 6cc2d8df57f7 · 2020-12-21T17:13:12.000+01:00
diff --git a/programs/sceneReconstruction/CMakeLists.txt b/programs/sceneReconstruction/CMakeLists.txt
@@ -24,8 +24,7 @@ if(ENABLE_sceneReconstruction)
                                               YARP::YARP_dev
                                               YARP::YARP_sig
                                               YARP::YARP_cv
-                                              opencv_rgbd
-                                              ROBOTICSLAB::ColorDebug)
+                                              opencv_rgbd)
 
     if(OpenCV_VERSION VERSION_GREATER_EQUAL 4.2)
         target_sources(sceneReconstruction PRIVATE DynaFu.cpp)
diff --git a/programs/sceneReconstruction/DynaFu.cpp b/programs/sceneReconstruction/DynaFu.cpp
@@ -2,9 +2,9 @@
 
 #include "KinectFusionImpl.hpp"
 
-#include <opencv2/rgbd/dynafu.hpp>
+#include <yarp/os/LogStream.h>
 
-#include <ColorDebug.h>
+#include <opencv2/rgbd/dynafu.hpp>
 
 namespace roboticslab
 {
@@ -15,10 +15,10 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co
 
     auto params = Params::defaultParams();
 
-    CD_INFO("algorithm: DynaFu\n");
+    yInfo() << "algorithm: DynaFu";
 
     params->frameSize = cv::Size(width, height);
-    CD_INFO("dimensions: width = %d, height = %d\n", width, height);
+    yInfo() << "dimensions: width =" << width << "x height =" << height;
 
     float fx = intrinsic.focalLengthX;
     float fy = intrinsic.focalLengthY;
@@ -29,7 +29,7 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co
                                 0, fy, cy,
                                 0,  0,  1);
 
-    CD_INFO("intrinsic params: fx = %f, fy = %f, cx = %f, cy = %f\n", fx, fy, cx, cy);
+    yInfo() << "intrinsic params: fx =" << fx << "fy =" << fy << "cx =" << cx << "cy =" << cy;
 
     updateParam(*params, &Params::depthFactor, config, "depthFactor", "pre-scale per 1 meter for input values");
     updateParam(*params, &Params::voxelSize, config, "voxelSize", "size of voxel in meters");
@@ -41,17 +41,17 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co
 
         if (volumeDims == nullptr || volumeDims->size() != 3)
         {
-            CD_ERROR("Parameter volumeDims must be a list of 3 integers.\n");
+            yError() << "Parameter volumeDims must be a list of 3 integers";
             return nullptr;
         }
 
         params->volumeDims = cv::Vec3i(volumeDims->get(0).asInt32(), volumeDims->get(1).asInt32(), volumeDims->get(2).asInt32());
-        CD_INFO("volumeDims: (%s)\n", volumeDims->toString().c_str());
+        yInfo() << "volumeDims =" << volumeDims->toString();
     }
     else
     {
         const auto & cvDims = params->volumeDims;
-        CD_INFO("volumeDims (DEFAULT): (%d %d %d)\n", cvDims[0], cvDims[1], cvDims[2]);
+        yInfo() << "volumeDims (DEFAULT) =" << cvDims[0] << cvDims[1] << cvDims[2];
     }
 
     if (config.check("lightPose", "light pose for rendering in meters"))
@@ -60,17 +60,17 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co
 
         if (lightPose == nullptr || lightPose->size() != 3)
         {
-            CD_ERROR("Parameter lightPose must be a list of 3 floats.\n");
+            yError() << "Parameter lightPose must be a list of 3 floats";
             return nullptr;
         }
 
         params->lightPose = cv::Vec3f(lightPose->get(0).asFloat32(), lightPose->get(1).asFloat32(), lightPose->get(2).asFloat32());
-        CD_INFO("lightPose: (%s)\n", lightPose->toString().c_str());
+        yInfo() << "lightPose =" << lightPose->toString();
     }
     else
     {
         const auto & cvLight = params->lightPose;
-        CD_INFO("lightPose (DEFAULT): (%f %f %f)\n", cvLight[0], cvLight[1], cvLight[2]);
+        yInfo() << "lightPose (DEFAULT) =" << cvLight[0] << cvLight[1] << cvLight[2];
     }
 
     if (config.check("volumePoseTransl", "volume pose (translation vector) in meters"))
@@ -79,18 +79,18 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co
 
         if (volumePoseTransl == nullptr || volumePoseTransl->size() != 3)
         {
-            CD_ERROR("Parameter volumePoseTransl must be a list of 3 floats.\n");
+            yError() << "Parameter volumePoseTransl must be a list of 3 floats";
             return nullptr;
         }
 
         auto transl = cv::Vec3f(volumePoseTransl->get(0).asFloat32(), volumePoseTransl->get(1).asFloat32(), volumePoseTransl->get(2).asFloat32());
         params->volumePose.translation(transl);
-        CD_INFO("volumePoseTransl: (%s)\n", volumePoseTransl->toString().c_str());
+        yInfo() << "volumePoseTransl =" << volumePoseTransl->toString();
     }
     else
     {
         const auto & transl = params->volumePose.translation();
-        CD_INFO("volumePoseTransl (DEFAULT): (%f %f %f)\n", transl[0], transl[1], transl[2]);
+        yInfo() << "volumePoseTransl (DEFAULT) =" << transl[0] << transl[1] << transl[2];
     }
 
     if (config.check("volumePoseRot", "volume pose (rotation matrix) in radians"))
@@ -99,7 +99,7 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co
 
         if (volumePoseRot == nullptr || volumePoseRot->size() != 9)
         {
-            CD_ERROR("Parameter volumePoseRot must be a list of 9 floats (3x3 matrix).\n");
+            yError() << "Parameter volumePoseRot must be a list of 9 floats (3x3 matrix)";
             return nullptr;
         }
 
@@ -108,12 +108,12 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co
                                volumePoseRot->get(6).asFloat32(), volumePoseRot->get(7).asFloat32(), volumePoseRot->get(8).asFloat32());
 
         params->volumePose.rotation(rot);
-        CD_INFO("volumePoseRot: (%s)\n", volumePoseRot->toString().c_str());
+        yInfo() << "volumePoseRot =" << volumePoseRot->toString();
     }
     else
     {
         const auto & rot = params->volumePose.rotation();
-        CD_INFO("volumePoseRot (DEFAULT): (%f %f %f %f %f %f %f %f %f)\n", rot(0,0), rot(0,1), rot(0,2), rot(1,0), rot(1,1), rot(1,2), rot(2,0), rot(2,1), rot(2,2));
+        yInfo() << "volumePoseRot (DEFAULT) =" << rot(0,0) << rot(0,1) << rot(0,2) << rot(1,0) << rot(1,1) << rot(1,2) << rot(2,0) << rot(2,1) << rot(2,2);
     }
 
     updateParam(*params, &Params::truncateThreshold, config, "truncateThreshold", "threshold for depth truncation in meters");
@@ -132,7 +132,7 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co
 
         if (icpIterations == nullptr)
         {
-            CD_ERROR("Parameter icpIterations must be a list.\n");
+            yError() << "Parameter icpIterations must be a list";
             return nullptr;
         }
 
@@ -143,11 +143,11 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co
             params->icpIterations[i] = icpIterations->get(i).asInt32();
         }
 
-        CD_INFO("icpIterations: (%s)\n", icpIterations->toString().c_str());
+        yInfo() << "icpIterations =" << icpIterations->toString();
     }
     else
     {
-        CD_INFO("icpIterations (DEFAULT): (%s)\n", vectorToString(params->icpIterations).c_str());
+        yInfo() << "icpIterations (DEFAULT) =" << params->icpIterations;
     }
 
     updateParam(*params, &Params::tsdf_max_weight, config, "tsdfMaxWeight", "max number of frames per voxel");
diff --git a/programs/sceneReconstruction/KinFu.cpp b/programs/sceneReconstruction/KinFu.cpp
@@ -4,11 +4,11 @@
 
 #include <map>
 
+#include <yarp/os/LogStream.h>
+
 #include <opencv2/core/version.hpp>
 #include <opencv2/rgbd/kinfu.hpp>
 
-#include <ColorDebug.h>
-
 #if CV_VERSION_MINOR >= 4
 namespace
 {
@@ -28,10 +28,10 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
 
     auto params = Params::defaultParams();
 
-    CD_INFO("algorithm: KinFu\n");
+    yInfo() << "algorithm: KinFu";
 
     params->frameSize = cv::Size(width, height);
-    CD_INFO("dimensions: width = %d, height = %d\n", width, height);
+    yInfo() << "dimensions: width =" << width << "x height =" << height;
 
     float fx = intrinsic.focalLengthX;
     float fy = intrinsic.focalLengthY;
@@ -42,7 +42,7 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
                                 0, fy, cy,
                                 0,  0,  1);
 
-    CD_INFO("intrinsic params: fx = %f, fy = %f, cx = %f, cy = %f\n", fx, fy, cx, cy);
+    yInfo() << "intrinsic params: fx =" << fx << "fy =" << fy << "cx =" << cx << "cy =" << cy;
 
     updateParam(*params, &Params::depthFactor, config, "depthFactor", "pre-scale per 1 meter for input values");
     updateParam(*params, &Params::voxelSize, config, "voxelSize", "size of voxel in meters");
@@ -54,17 +54,17 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
 
         if (volumeDims == nullptr || volumeDims->size() != 3)
         {
-            CD_ERROR("Parameter volumeDims must be a list of 3 integers.\n");
+            yError() << "Parameter volumeDims must be a list of 3 integers";
             return nullptr;
         }
 
         params->volumeDims = cv::Vec3i(volumeDims->get(0).asInt32(), volumeDims->get(1).asInt32(), volumeDims->get(2).asInt32());
-        CD_INFO("volumeDims: (%s)\n", volumeDims->toString().c_str());
+        yInfo() << "volumeDims =" << volumeDims->toString();
     }
     else
     {
         const auto & cvDims = params->volumeDims;
-        CD_INFO("volumeDims (DEFAULT): (%d %d %d)\n", cvDims[0], cvDims[1], cvDims[2]);
+        yInfo() << "volumeDims (DEFAULT) =" << cvDims[0] << cvDims[1] << cvDims[2];
     }
 
     if (config.check("lightPose", "light pose for rendering in meters"))
@@ -73,17 +73,17 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
 
         if (lightPose == nullptr || lightPose->size() != 3)
         {
-            CD_ERROR("Parameter lightPose must be a list of 3 floats.\n");
+            yError() << "Parameter lightPose must be a list of 3 floats";
             return nullptr;
         }
 
         params->lightPose = cv::Vec3f(lightPose->get(0).asFloat32(), lightPose->get(1).asFloat32(), lightPose->get(2).asFloat32());
-        CD_INFO("lightPose: (%s)\n", lightPose->toString().c_str());
+        yInfo() << "lightPose =" << lightPose->toString();
     }
     else
     {
         const auto & cvLight = params->lightPose;
-        CD_INFO("lightPose (DEFAULT): (%f %f %f)\n", cvLight[0], cvLight[1], cvLight[2]);
+        yInfo() << "lightPose (DEFAULT) =" << cvLight[0] << cvLight[1] << cvLight[2];
     }
 
     if (config.check("volumePoseTransl", "volume pose (translation vector) in meters"))
@@ -92,18 +92,18 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
 
         if (volumePoseTransl == nullptr || volumePoseTransl->size() != 3)
         {
-            CD_ERROR("Parameter volumePoseTransl must be a list of 3 floats.\n");
+            yError() << "Parameter volumePoseTransl must be a list of 3 floats";
             return nullptr;
         }
 
         auto transl = cv::Vec3f(volumePoseTransl->get(0).asFloat32(), volumePoseTransl->get(1).asFloat32(), volumePoseTransl->get(2).asFloat32());
         params->volumePose.translation(transl);
-        CD_INFO("volumePoseTransl: (%s)\n", volumePoseTransl->toString().c_str());
+        yInfo() << "volumePoseTransl =" << volumePoseTransl->toString();
     }
     else
     {
         const auto & transl = params->volumePose.translation();
-        CD_INFO("volumePoseTransl (DEFAULT): (%f %f %f)\n", transl[0], transl[1], transl[2]);
+        yInfo() << "volumePoseTransl (DEFAULT) =" << transl[0] << transl[1] << transl[2];
     }
 
     if (config.check("volumePoseRot", "volume pose (rotation matrix) in radians"))
@@ -112,7 +112,7 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
 
         if (volumePoseRot == nullptr || volumePoseRot->size() != 9)
         {
-            CD_ERROR("Parameter volumePoseRot must be a list of 9 floats (3x3 matrix).\n");
+            yError() << "Parameter volumePoseRot must be a list of 9 floats (3x3 matrix)";
             return nullptr;
         }
 
@@ -121,12 +121,12 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
                                volumePoseRot->get(6).asFloat32(), volumePoseRot->get(7).asFloat32(), volumePoseRot->get(8).asFloat32());
 
         params->volumePose.rotation(rot);
-        CD_INFO("volumePoseRot: (%s)\n", volumePoseRot->toString().c_str());
+        yInfo() << "volumePoseRot =" << volumePoseRot->toString();
     }
     else
     {
         const auto & rot = params->volumePose.rotation();
-        CD_INFO("volumePoseRot (DEFAULT): (%f %f %f %f %f %f %f %f %f)\n", rot(0,0), rot(0,1), rot(0,2), rot(1,0), rot(1,1), rot(1,2), rot(2,0), rot(2,1), rot(2,2));
+        yInfo() << "volumePoseRot (DEFAULT) =" << rot(0,0) << rot(0,1) << rot(0,2) << rot(1,0) << rot(1,1) << rot(1,2) << rot(2,0) << rot(2,1) << rot(2,2);
     }
 
 #if CV_VERSION_MINOR >= 2
@@ -140,17 +140,17 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
 
         if (stringToCvVolume.find(volumeType) == stringToCvVolume.end())
         {
-            CD_ERROR("Unsupported volume type: %s.\n", volumeType.c_str());
+            yError() << "Unsupported volume type" << volumeType;
             return nullptr;
         }
 
         params->volumeType = stringToCvVolume[volumeType];
-        CD_INFO("volumeType: %s\n", volumeType.c_str());
+        yInfo() << "volumeType =" << volumeType;
     }
     else
     {
         auto res = std::find_if(stringToCvVolume.begin(), stringToCvVolume.end(), [&params](const auto & el) { return el.second == params->volumeType; });
-        CD_INFO("volumeType (DEFAULT): %s\n", res->first.c_str());
+        yInfo() << "volumeType (DEFAULT) =" << res->first;
     }
 #endif
 
@@ -168,7 +168,7 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
 
         if (icpIterations == nullptr)
         {
-            CD_ERROR("Parameter icpIterations must be a list.\n");
+            yError() << "Parameter icpIterations must be a list";
             return nullptr;
         }
 
@@ -179,11 +179,11 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
             params->icpIterations[i] = icpIterations->get(i).asInt32();
         }
 
-        CD_INFO("icpIterations: (%s)\n", icpIterations->toString().c_str());
+        yInfo() << "icpIterations =" << icpIterations->toString();
     }
     else
     {
-        CD_INFO("icpIterations (DEFAULT): (%s)\n", vectorToString(params->icpIterations).c_str());
+        yInfo() << "icpIterations (DEFAULT) =" << params->icpIterations;
     }
 
     updateParam(*params, &Params::tsdf_max_weight, config, "tsdfMaxWeight", "max number of frames per voxel");
diff --git a/programs/sceneReconstruction/KinectFusionImpl.hpp b/programs/sceneReconstruction/KinectFusionImpl.hpp
@@ -5,15 +5,11 @@
 
 #include "KinectFusion.hpp"
 
-#include <algorithm>
-#include <iterator>
-#include <sstream>
 #include <type_traits>
 
+#include <yarp/os/LogStream.h>
 #include <yarp/cv/Cv.h>
 
-#include <ColorDebug.h>
-
 namespace roboticslab
 {
 
@@ -112,31 +108,19 @@ template <typename TParams, typename TRet>
 void updateParam(TParams & params, TRet TParams::* param, const yarp::os::Searchable & config,
     const std::string & name, const std::string & description)
 {
-    std::stringstream ss;
-    ss << name;
+    auto && log = yInfo();
+    log << name;
 
     if (config.check(name, description))
     {
         params.*param = getValue<TRet>(config.find(name));
-        ss << ": ";
     }
     else
     {
-        ss << " (DEFAULT): ";
+        log << "(DEFAULT)";
     }
 
-    ss << params.*param << "\n";
-    CD_INFO(ss.str().c_str());
-}
-
-template <typename T>
-inline std::string vectorToString(const std::vector<T> & vec, const std::string & delimiter = " ")
-{
-    // https://stackoverflow.com/a/8581865
-    std::ostringstream oss;
-    std::copy(vec.begin(), vec.end() - 1, std::ostream_iterator<T>(oss, delimiter.c_str()));
-    oss << vec.back();
-    return oss.str();
+    log << "=" << params.*param;
 }
 
 } // namespace roboticslab
diff --git a/programs/sceneReconstruction/SceneReconstruction.cpp b/programs/sceneReconstruction/SceneReconstruction.cpp
diff --git a/programs/sceneReconstruction/main.cpp b/programs/sceneReconstruction/main.cpp

Original file line number	Diff line number	Diff line change
`@@ -2,9 +2,9 @@`
`2`	`2`
`3`	`3`	`#include "KinectFusionImpl.hpp"`
`4`	`4`
`5`		`-#include <opencv2/rgbd/dynafu.hpp>`
	`5`	`+#include <yarp/os/LogStream.h>`
`6`	`6`
`7`		`-#include <ColorDebug.h>`
	`7`	`+#include <opencv2/rgbd/dynafu.hpp>`
`8`	`8`
`9`	`9`	`namespace roboticslab`
`10`	`10`	`{`
`@@ -15,10 +15,10 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co`
`15`	`15`
`16`	`16`	`auto params = Params::defaultParams();`
`17`	`17`
`18`		`- CD_INFO("algorithm: DynaFu\n");`
	`18`	`+ yInfo() << "algorithm: DynaFu";`
`19`	`19`
`20`	`20`	`params->frameSize = cv::Size(width, height);`
`21`		`- CD_INFO("dimensions: width = %d, height = %d\n", width, height);`
	`21`	`+ yInfo() << "dimensions: width =" << width << "x height =" << height;`
`22`	`22`
`23`	`23`	`float fx = intrinsic.focalLengthX;`
`24`	`24`	`float fy = intrinsic.focalLengthY;`
`@@ -29,7 +29,7 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co`
`29`	`29`	`0, fy, cy,`
`30`	`30`	`0, 0, 1);`
`31`	`31`
`32`		`- CD_INFO("intrinsic params: fx = %f, fy = %f, cx = %f, cy = %f\n", fx, fy, cx, cy);`
	`32`	`+ yInfo() << "intrinsic params: fx =" << fx << "fy =" << fy << "cx =" << cx << "cy =" << cy;`
`33`	`33`
`34`	`34`	`updateParam(*params, &Params::depthFactor, config, "depthFactor", "pre-scale per 1 meter for input values");`
`35`	`35`	`updateParam(*params, &Params::voxelSize, config, "voxelSize", "size of voxel in meters");`
`@@ -41,17 +41,17 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co`
`41`	`41`
`42`	`42`	`if (volumeDims == nullptr \|\| volumeDims->size() != 3)`
`43`	`43`	`{`
`44`		`- CD_ERROR("Parameter volumeDims must be a list of 3 integers.\n");`
	`44`	`+ yError() << "Parameter volumeDims must be a list of 3 integers";`
`45`	`45`	`return nullptr;`
`46`	`46`	`}`
`47`	`47`
`48`	`48`	`params->volumeDims = cv::Vec3i(volumeDims->get(0).asInt32(), volumeDims->get(1).asInt32(), volumeDims->get(2).asInt32());`
`49`		`- CD_INFO("volumeDims: (%s)\n", volumeDims->toString().c_str());`
	`49`	`+ yInfo() << "volumeDims =" << volumeDims->toString();`
`50`	`50`	`}`
`51`	`51`	`else`
`52`	`52`	`{`
`53`	`53`	`const auto & cvDims = params->volumeDims;`
`54`		`- CD_INFO("volumeDims (DEFAULT): (%d %d %d)\n", cvDims[0], cvDims[1], cvDims[2]);`
	`54`	`+ yInfo() << "volumeDims (DEFAULT) =" << cvDims[0] << cvDims[1] << cvDims[2];`
`55`	`55`	`}`
`56`	`56`
`57`	`57`	`if (config.check("lightPose", "light pose for rendering in meters"))`
`@@ -60,17 +60,17 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co`
`60`	`60`
`61`	`61`	`if (lightPose == nullptr \|\| lightPose->size() != 3)`
`62`	`62`	`{`
`63`		`- CD_ERROR("Parameter lightPose must be a list of 3 floats.\n");`
	`63`	`+ yError() << "Parameter lightPose must be a list of 3 floats";`
`64`	`64`	`return nullptr;`
`65`	`65`	`}`
`66`	`66`
`67`	`67`	`params->lightPose = cv::Vec3f(lightPose->get(0).asFloat32(), lightPose->get(1).asFloat32(), lightPose->get(2).asFloat32());`
`68`		`- CD_INFO("lightPose: (%s)\n", lightPose->toString().c_str());`
	`68`	`+ yInfo() << "lightPose =" << lightPose->toString();`
`69`	`69`	`}`
`70`	`70`	`else`
`71`	`71`	`{`
`72`	`72`	`const auto & cvLight = params->lightPose;`
`73`		`- CD_INFO("lightPose (DEFAULT): (%f %f %f)\n", cvLight[0], cvLight[1], cvLight[2]);`
	`73`	`+ yInfo() << "lightPose (DEFAULT) =" << cvLight[0] << cvLight[1] << cvLight[2];`
`74`	`74`	`}`
`75`	`75`
`76`	`76`	`if (config.check("volumePoseTransl", "volume pose (translation vector) in meters"))`
`@@ -79,18 +79,18 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co`
`79`	`79`
`80`	`80`	`if (volumePoseTransl == nullptr \|\| volumePoseTransl->size() != 3)`
`81`	`81`	`{`
`82`		`- CD_ERROR("Parameter volumePoseTransl must be a list of 3 floats.\n");`
	`82`	`+ yError() << "Parameter volumePoseTransl must be a list of 3 floats";`
`83`	`83`	`return nullptr;`
`84`	`84`	`}`
`85`	`85`
`86`	`86`	`auto transl = cv::Vec3f(volumePoseTransl->get(0).asFloat32(), volumePoseTransl->get(1).asFloat32(), volumePoseTransl->get(2).asFloat32());`
`87`	`87`	`params->volumePose.translation(transl);`
`88`		`- CD_INFO("volumePoseTransl: (%s)\n", volumePoseTransl->toString().c_str());`
	`88`	`+ yInfo() << "volumePoseTransl =" << volumePoseTransl->toString();`
`89`	`89`	`}`
`90`	`90`	`else`
`91`	`91`	`{`
`92`	`92`	`const auto & transl = params->volumePose.translation();`
`93`		`- CD_INFO("volumePoseTransl (DEFAULT): (%f %f %f)\n", transl[0], transl[1], transl[2]);`
	`93`	`+ yInfo() << "volumePoseTransl (DEFAULT) =" << transl[0] << transl[1] << transl[2];`
`94`	`94`	`}`
`95`	`95`
`96`	`96`	`if (config.check("volumePoseRot", "volume pose (rotation matrix) in radians"))`
`@@ -99,7 +99,7 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co`
`99`	`99`
`100`	`100`	`if (volumePoseRot == nullptr \|\| volumePoseRot->size() != 9)`
`101`	`101`	`{`
`102`		`- CD_ERROR("Parameter volumePoseRot must be a list of 9 floats (3x3 matrix).\n");`
	`102`	`+ yError() << "Parameter volumePoseRot must be a list of 9 floats (3x3 matrix)";`
`103`	`103`	`return nullptr;`
`104`	`104`	`}`
`105`	`105`
`@@ -108,12 +108,12 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co`
`108`	`108`	`volumePoseRot->get(6).asFloat32(), volumePoseRot->get(7).asFloat32(), volumePoseRot->get(8).asFloat32());`
`109`	`109`
`110`	`110`	`params->volumePose.rotation(rot);`
`111`		`- CD_INFO("volumePoseRot: (%s)\n", volumePoseRot->toString().c_str());`
	`111`	`+ yInfo() << "volumePoseRot =" << volumePoseRot->toString();`
`112`	`112`	`}`
`113`	`113`	`else`
`114`	`114`	`{`
`115`	`115`	`const auto & rot = params->volumePose.rotation();`
`116`		`- CD_INFO("volumePoseRot (DEFAULT): (%f %f %f %f %f %f %f %f %f)\n", rot(0,0), rot(0,1), rot(0,2), rot(1,0), rot(1,1), rot(1,2), rot(2,0), rot(2,1), rot(2,2));`
	`116`	`+ yInfo() << "volumePoseRot (DEFAULT) =" << rot(0,0) << rot(0,1) << rot(0,2) << rot(1,0) << rot(1,1) << rot(1,2) << rot(2,0) << rot(2,1) << rot(2,2);`
`117`	`117`	`}`
`118`	`118`
`119`	`119`	`updateParam(*params, &Params::truncateThreshold, config, "truncateThreshold", "threshold for depth truncation in meters");`
`@@ -132,7 +132,7 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co`
`132`	`132`
`133`	`133`	`if (icpIterations == nullptr)`
`134`	`134`	`{`
`135`		`- CD_ERROR("Parameter icpIterations must be a list.\n");`
	`135`	`+ yError() << "Parameter icpIterations must be a list";`
`136`	`136`	`return nullptr;`
`137`	`137`	`}`
`138`	`138`
`@@ -143,11 +143,11 @@ std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, co`
`143`	`143`	`params->icpIterations[i] = icpIterations->get(i).asInt32();`
`144`	`144`	`}`
`145`	`145`
`146`		`- CD_INFO("icpIterations: (%s)\n", icpIterations->toString().c_str());`
	`146`	`+ yInfo() << "icpIterations =" << icpIterations->toString();`
`147`	`147`	`}`
`148`	`148`	`else`
`149`	`149`	`{`
`150`		`- CD_INFO("icpIterations (DEFAULT): (%s)\n", vectorToString(params->icpIterations).c_str());`
	`150`	`+ yInfo() << "icpIterations (DEFAULT) =" << params->icpIterations;`
`151`	`151`	`}`
`152`	`152`
`153`	`153`	`updateParam(*params, &Params::tsdf_max_weight, config, "tsdfMaxWeight", "max number of frames per voxel");`
Original file line number	Diff line number	Diff line change
`@@ -4,11 +4,11 @@`
`4`	`4`
`5`	`5`	`#include <map>`
`6`	`6`
	`7`	`+#include <yarp/os/LogStream.h>`
	`8`	`+`
`7`	`9`	`#include <opencv2/core/version.hpp>`
`8`	`10`	`#include <opencv2/rgbd/kinfu.hpp>`
`9`	`11`
`10`		`-#include <ColorDebug.h>`
`11`		`-`
`12`	`12`	`#if CV_VERSION_MINOR >= 4`
`13`	`13`	`namespace`
`14`	`14`	`{`
`@@ -28,10 +28,10 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con`
`28`	`28`
`29`	`29`	`auto params = Params::defaultParams();`
`30`	`30`
`31`		`- CD_INFO("algorithm: KinFu\n");`
	`31`	`+ yInfo() << "algorithm: KinFu";`
`32`	`32`
`33`	`33`	`params->frameSize = cv::Size(width, height);`
`34`		`- CD_INFO("dimensions: width = %d, height = %d\n", width, height);`
	`34`	`+ yInfo() << "dimensions: width =" << width << "x height =" << height;`
`35`	`35`
`36`	`36`	`float fx = intrinsic.focalLengthX;`
`37`	`37`	`float fy = intrinsic.focalLengthY;`
`@@ -42,7 +42,7 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con`
`42`	`42`	`0, fy, cy,`
`43`	`43`	`0, 0, 1);`
`44`	`44`
`45`		`- CD_INFO("intrinsic params: fx = %f, fy = %f, cx = %f, cy = %f\n", fx, fy, cx, cy);`
	`45`	`+ yInfo() << "intrinsic params: fx =" << fx << "fy =" << fy << "cx =" << cx << "cy =" << cy;`
`46`	`46`
`47`	`47`	`updateParam(*params, &Params::depthFactor, config, "depthFactor", "pre-scale per 1 meter for input values");`
`48`	`48`	`updateParam(*params, &Params::voxelSize, config, "voxelSize", "size of voxel in meters");`
`@@ -54,17 +54,17 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con`
`54`	`54`
`55`	`55`	`if (volumeDims == nullptr \|\| volumeDims->size() != 3)`
`56`	`56`	`{`
`57`		`- CD_ERROR("Parameter volumeDims must be a list of 3 integers.\n");`
	`57`	`+ yError() << "Parameter volumeDims must be a list of 3 integers";`
`58`	`58`	`return nullptr;`
`59`	`59`	`}`
`60`	`60`
`61`	`61`	`params->volumeDims = cv::Vec3i(volumeDims->get(0).asInt32(), volumeDims->get(1).asInt32(), volumeDims->get(2).asInt32());`
`62`		`- CD_INFO("volumeDims: (%s)\n", volumeDims->toString().c_str());`
	`62`	`+ yInfo() << "volumeDims =" << volumeDims->toString();`
`63`	`63`	`}`
`64`	`64`	`else`
`65`	`65`	`{`
`66`	`66`	`const auto & cvDims = params->volumeDims;`
`67`		`- CD_INFO("volumeDims (DEFAULT): (%d %d %d)\n", cvDims[0], cvDims[1], cvDims[2]);`
	`67`	`+ yInfo() << "volumeDims (DEFAULT) =" << cvDims[0] << cvDims[1] << cvDims[2];`
`68`	`68`	`}`
`69`	`69`
`70`	`70`	`if (config.check("lightPose", "light pose for rendering in meters"))`
`@@ -73,17 +73,17 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con`
`73`	`73`
`74`	`74`	`if (lightPose == nullptr \|\| lightPose->size() != 3)`
`75`	`75`	`{`
`76`		`- CD_ERROR("Parameter lightPose must be a list of 3 floats.\n");`
	`76`	`+ yError() << "Parameter lightPose must be a list of 3 floats";`
`77`	`77`	`return nullptr;`
`78`	`78`	`}`
`79`	`79`
`80`	`80`	`params->lightPose = cv::Vec3f(lightPose->get(0).asFloat32(), lightPose->get(1).asFloat32(), lightPose->get(2).asFloat32());`
`81`		`- CD_INFO("lightPose: (%s)\n", lightPose->toString().c_str());`
	`81`	`+ yInfo() << "lightPose =" << lightPose->toString();`
`82`	`82`	`}`
`83`	`83`	`else`
`84`	`84`	`{`
`85`	`85`	`const auto & cvLight = params->lightPose;`
`86`		`- CD_INFO("lightPose (DEFAULT): (%f %f %f)\n", cvLight[0], cvLight[1], cvLight[2]);`
	`86`	`+ yInfo() << "lightPose (DEFAULT) =" << cvLight[0] << cvLight[1] << cvLight[2];`
`87`	`87`	`}`
`88`	`88`
`89`	`89`	`if (config.check("volumePoseTransl", "volume pose (translation vector) in meters"))`
`@@ -92,18 +92,18 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con`
`92`	`92`
`93`	`93`	`if (volumePoseTransl == nullptr \|\| volumePoseTransl->size() != 3)`
`94`	`94`	`{`
`95`		`- CD_ERROR("Parameter volumePoseTransl must be a list of 3 floats.\n");`
	`95`	`+ yError() << "Parameter volumePoseTransl must be a list of 3 floats";`
`96`	`96`	`return nullptr;`
`97`	`97`	`}`
`98`	`98`
`99`	`99`	`auto transl = cv::Vec3f(volumePoseTransl->get(0).asFloat32(), volumePoseTransl->get(1).asFloat32(), volumePoseTransl->get(2).asFloat32());`
`100`	`100`	`params->volumePose.translation(transl);`
`101`		`- CD_INFO("volumePoseTransl: (%s)\n", volumePoseTransl->toString().c_str());`
	`101`	`+ yInfo() << "volumePoseTransl =" << volumePoseTransl->toString();`
`102`	`102`	`}`
`103`	`103`	`else`
`104`	`104`	`{`
`105`	`105`	`const auto & transl = params->volumePose.translation();`
`106`		`- CD_INFO("volumePoseTransl (DEFAULT): (%f %f %f)\n", transl[0], transl[1], transl[2]);`
	`106`	`+ yInfo() << "volumePoseTransl (DEFAULT) =" << transl[0] << transl[1] << transl[2];`
`107`	`107`	`}`
`108`	`108`
`109`	`109`	`if (config.check("volumePoseRot", "volume pose (rotation matrix) in radians"))`
`@@ -112,7 +112,7 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con`
`112`	`112`
`113`	`113`	`if (volumePoseRot == nullptr \|\| volumePoseRot->size() != 9)`
`114`	`114`	`{`
`115`		`- CD_ERROR("Parameter volumePoseRot must be a list of 9 floats (3x3 matrix).\n");`
	`115`	`+ yError() << "Parameter volumePoseRot must be a list of 9 floats (3x3 matrix)";`
`116`	`116`	`return nullptr;`
`117`	`117`	`}`
`118`	`118`
`@@ -121,12 +121,12 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con`
`121`	`121`	`volumePoseRot->get(6).asFloat32(), volumePoseRot->get(7).asFloat32(), volumePoseRot->get(8).asFloat32());`
`122`	`122`
`123`	`123`	`params->volumePose.rotation(rot);`
`124`		`- CD_INFO("volumePoseRot: (%s)\n", volumePoseRot->toString().c_str());`
	`124`	`+ yInfo() << "volumePoseRot =" << volumePoseRot->toString();`
`125`	`125`	`}`
`126`	`126`	`else`
`127`	`127`	`{`
`128`	`128`	`const auto & rot = params->volumePose.rotation();`
`129`		`- CD_INFO("volumePoseRot (DEFAULT): (%f %f %f %f %f %f %f %f %f)\n", rot(0,0), rot(0,1), rot(0,2), rot(1,0), rot(1,1), rot(1,2), rot(2,0), rot(2,1), rot(2,2));`
	`129`	`+ yInfo() << "volumePoseRot (DEFAULT) =" << rot(0,0) << rot(0,1) << rot(0,2) << rot(1,0) << rot(1,1) << rot(1,2) << rot(2,0) << rot(2,1) << rot(2,2);`
`130`	`130`	`}`
`131`	`131`
`132`	`132`	`#if CV_VERSION_MINOR >= 2`
`@@ -140,17 +140,17 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con`
`140`	`140`
`141`	`141`	`if (stringToCvVolume.find(volumeType) == stringToCvVolume.end())`
`142`	`142`	`{`
`143`		`- CD_ERROR("Unsupported volume type: %s.\n", volumeType.c_str());`
	`143`	`+ yError() << "Unsupported volume type" << volumeType;`
`144`	`144`	`return nullptr;`
`145`	`145`	`}`
`146`	`146`
`147`	`147`	`params->volumeType = stringToCvVolume[volumeType];`
`148`		`- CD_INFO("volumeType: %s\n", volumeType.c_str());`
	`148`	`+ yInfo() << "volumeType =" << volumeType;`
`149`	`149`	`}`
`150`	`150`	`else`
`151`	`151`	`{`
`152`	`152`	`auto res = std::find_if(stringToCvVolume.begin(), stringToCvVolume.end(), [&params](const auto & el) { return el.second == params->volumeType; });`
`153`		`- CD_INFO("volumeType (DEFAULT): %s\n", res->first.c_str());`
	`153`	`+ yInfo() << "volumeType (DEFAULT) =" << res->first;`
`154`	`154`	`}`
`155`	`155`	`#endif`
`156`	`156`
`@@ -168,7 +168,7 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con`
`168`	`168`
`169`	`169`	`if (icpIterations == nullptr)`
`170`	`170`	`{`
`171`		`- CD_ERROR("Parameter icpIterations must be a list.\n");`
	`171`	`+ yError() << "Parameter icpIterations must be a list";`
`172`	`172`	`return nullptr;`
`173`	`173`	`}`
`174`	`174`
`@@ -179,11 +179,11 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con`
`179`	`179`	`params->icpIterations[i] = icpIterations->get(i).asInt32();`
`180`	`180`	`}`
`181`	`181`
`182`		`- CD_INFO("icpIterations: (%s)\n", icpIterations->toString().c_str());`
	`182`	`+ yInfo() << "icpIterations =" << icpIterations->toString();`
`183`	`183`	`}`
`184`	`184`	`else`
`185`	`185`	`{`
`186`		`- CD_INFO("icpIterations (DEFAULT): (%s)\n", vectorToString(params->icpIterations).c_str());`
	`186`	`+ yInfo() << "icpIterations (DEFAULT) =" << params->icpIterations;`
`187`	`187`	`}`
`188`	`188`
`189`	`189`	`updateParam(*params, &Params::tsdf_max_weight, config, "tsdfMaxWeight", "max number of frames per voxel");`