diff --git a/MotionPlanning/CSpace/CSpace.cpp b/MotionPlanning/CSpace/CSpace.cpp
index 40a496b56cfa381089e7739f5f842b3867c64dd4..0113fea50805170b06cd059f019b188ab8efb382 100644
--- a/MotionPlanning/CSpace/CSpace.cpp
+++ b/MotionPlanning/CSpace/CSpace.cpp
@@ -406,7 +406,7 @@ namespace Saba
         return d;
     }
 
-    float CSpace::getDirectedMaxMovement(const Eigen::VectorXf& config, const Eigen::VectorXf& nextConfig)
+    float CSpace::getDirectedMaxMovement(const Eigen::VectorXf& /*config*/, const Eigen::VectorXf& nextConfig)
     {
         SABA_ASSERT(config.rows() == dimension)
         SABA_ASSERT(nextConfig.rows() == dimension)
diff --git a/VirtualRobot/IK/PoseQualityMeasurement.cpp b/VirtualRobot/IK/PoseQualityMeasurement.cpp
index fa1d0116d0aeee89cdbd14ba3bab189c91efad66..c6c19271e6908f646a3d4be82d379bd0cd7f2779 100644
--- a/VirtualRobot/IK/PoseQualityMeasurement.cpp
+++ b/VirtualRobot/IK/PoseQualityMeasurement.cpp
@@ -29,7 +29,7 @@ namespace VirtualRobot
         return 0.0f;
     }
 
-    float PoseQualityMeasurement::getPoseQuality(const Eigen::VectorXf& direction)
+    float PoseQualityMeasurement::getPoseQuality(const Eigen::VectorXf& /*direction*/)
     {
         VR_ASSERT(direction.rows() == 3 || direction.rows() == 6);
         VR_WARNING << "Please use derived classes..." << endl;
diff --git a/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp b/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
index ce6bbc4c59eb14a3485816054180ed0133809bd0..93684b693104bdf8e5809bbabb63b0ab754d86b1 100644
--- a/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
+++ b/VirtualRobot/Import/URDF/SimoxURDFFactory.cpp
@@ -31,7 +31,7 @@ namespace VirtualRobot
     {
     }
 
-    RobotPtr SimoxURDFFactory::loadFromFile(const std::string& filename, RobotIO::RobotDescription loadMode)
+    RobotPtr SimoxURDFFactory::loadFromFile(const std::string& filename, RobotIO::RobotDescription)
     {
 
         RobotPtr result;
@@ -313,7 +313,6 @@ namespace VirtualRobot
 
     RobotNodePtr SimoxURDFFactory::createBodyNode(RobotPtr robo, std::shared_ptr<Link> urdfBody, const std::string& basePath, bool useColModelsIfNoVisuModel)
     {
-        const float scale = 1000.0f; // mm
         RobotNodePtr result;
 
         if (!urdfBody)
@@ -389,7 +388,6 @@ namespace VirtualRobot
             physics.localCoM = convertPose(urdfBody->inertial->origin).block(0, 3, 3, 1);
         }
 
-        Eigen::Matrix4f idMatrix = Eigen::Matrix4f::Identity();
         Eigen::Vector3f idVec3 = Eigen::Vector3f::Zero();
         result = fixedNodeFactory->createRobotNode(robo, name, rnVisu, rnCol, 0, 0, 0, preJointTransform, idVec3, idVec3, physics);
 
@@ -400,7 +398,6 @@ namespace VirtualRobot
 
     RobotNodePtr SimoxURDFFactory::createJointNode(RobotPtr robo, std::shared_ptr<Joint> urdfJoint)
     {
-        const float scale = 1000.0f; // mm
         RobotNodePtr result;
 
         if (!urdfJoint)
@@ -412,7 +409,6 @@ namespace VirtualRobot
         VirtualRobot::RobotNodeFactoryPtr revoluteNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeRevoluteFactory::getName(), NULL);
         VirtualRobot::RobotNodeFactoryPtr fixedNodeFactory = VirtualRobot::RobotNodeFactory::fromName(VirtualRobot::RobotNodeFixedFactory::getName(), NULL);
 
-        Eigen::Matrix4f idMatrix = Eigen::Matrix4f::Identity();
         Eigen::Vector3f idVec3 = Eigen::Vector3f::Zero();
         std::string name = urdfJoint->name;
 
diff --git a/VirtualRobot/MathTools.h b/VirtualRobot/MathTools.h
index d3ed32aeca6cca367e6efd959eda9833f198d8e1..f4359124d3dd0b02acb33386a69be042ddce9ba1 100644
--- a/VirtualRobot/MathTools.h
+++ b/VirtualRobot/MathTools.h
@@ -399,11 +399,7 @@ namespace VirtualRobot
 
         struct TriangleFace
         {
-            TriangleFace()
-                : id1(UINT_MAX), id2(UINT_MAX), id3(UINT_MAX),
-                  idColor1(UINT_MAX), idColor2(UINT_MAX), idColor3(UINT_MAX),
-                  idNormal1(UINT_MAX), idNormal2(UINT_MAX), idNormal3(UINT_MAX),
-                  idMaterial(UINT_MAX) {}
+            TriangleFace() = default;
 
             /**
              * Flips the orientation of the contained vertex and the normal.
@@ -437,25 +433,25 @@ namespace VirtualRobot
             }
 
             // id == position in vertex array
-            unsigned int id1;
-            unsigned int id2;
-            unsigned int id3;
+            unsigned int id1{UINT_MAX};
+            unsigned int id2{UINT_MAX};
+            unsigned int id3{UINT_MAX};
 
             // idColor == position in color array
-            unsigned int idColor1;
-            unsigned int idColor2;
-            unsigned int idColor3;
+            unsigned int idColor1{UINT_MAX};
+            unsigned int idColor2{UINT_MAX};
+            unsigned int idColor3{UINT_MAX};
 
             //idNormal == position in normal array
-            unsigned int idNormal1;
-            unsigned int idNormal2;
-            unsigned int idNormal3;
+            unsigned int idNormal1{UINT_MAX};
+            unsigned int idNormal2{UINT_MAX};
+            unsigned int idNormal3{UINT_MAX};
 
             // idMaterial == position in material array
-            unsigned int idMaterial;
+            unsigned int idMaterial{UINT_MAX};
 
             // per face normal (used when no idNormals are given)
-            Eigen::Vector3f normal;
+            Eigen::Vector3f normal{0, 0, 0};
         };
         struct TriangleFace6D
         {
diff --git a/VirtualRobot/Visualization/VisualizationFactory.h b/VirtualRobot/Visualization/VisualizationFactory.h
index cd416a0394633ad421469fe09468437e119b5c9a..5e2ab814c2db0a32b00999c46e1de1514da98ffd 100644
--- a/VirtualRobot/Visualization/VisualizationFactory.h
+++ b/VirtualRobot/Visualization/VisualizationFactory.h
@@ -42,14 +42,10 @@ namespace VirtualRobot
 
         struct Color
         {
-            Color()
-            {
-                transparency = 0.0f;
-                r = g = b = 0.5f;
-            }
+            Color() = default;
             Color(float r, float g, float b, float transparency = 0.0f): r(r), g(g), b(b), transparency(transparency) {}
-            float r, g, b;
-            float transparency;
+            float r = 0.5f, g = 0.5f, b = 0.5f;
+            float transparency = 1;
             bool isNone() const
             {
                 return transparency >= 1.0f;
@@ -82,107 +78,101 @@ namespace VirtualRobot
 
         struct PhongMaterial
         {
-            PhongMaterial() {}
+            PhongMaterial() = default;
             Color emission;
             Color ambient;
             Color diffuse;
             Color specular;
-            float shininess;
+            float shininess{0};
             Color reflective;
-            float reflectivity;
+            float reflectivity{0};
             Color transparent;
-            float transparency;
-            float refractionIndex;
+            float transparency{0};
+            float refractionIndex{0};
         };
+        
+        VisualizationFactory() = default;
+        virtual ~VisualizationFactory() = default;
 
-        VisualizationFactory()
-        {
-            ;
-        }
-        virtual ~VisualizationFactory()
-        {
-            ;
-        }
-
-        virtual void init(int &/*argc*/, char* /*argv*/[], const std::string &/*appName*/)
+        virtual void init(int& /*argc*/, char* /*argv*/[], const std::string& /*appName*/)
         {
         }
 
         virtual VisualizationNodePtr getVisualizationFromPrimitives(const std::vector<Primitive::PrimitivePtr>& /*primitives*/, bool /*boundingBox*/ = false, Color /*color*/ = Color::Gray())
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr getVisualizationFromFile(const std::string& /*filename*/, bool /*boundingBox*/ = false, float /*scaleX*/ = 1.0f, float /*scaleY*/ = 1.0f, float /*scaleZ*/ = 1.0f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr getVisualizationFromFile(const std::ifstream& /*ifs*/, bool /*boundingBox*/ = false, float /*scaleX*/ = 1.0f, float /*scaleY*/ = 1.0f, float /*scaleZ*/ = 1.0f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         /*!
             A box, dimensions are given in mm.
         */
         virtual VisualizationNodePtr createBox(float /*width*/, float /*height*/, float /*depth*/, float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createLine(const Eigen::Vector3f& /*from*/, const Eigen::Vector3f& /*to*/, float /*width*/ = 1.0f, float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createLine(const Eigen::Matrix4f& /*from*/, const Eigen::Matrix4f& /*to*/, float /*width*/ = 1.0f, float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createSphere(float /*radius*/, float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createCircle(float /*radius*/, float /*circleCompletion*/, float /*width*/, float /*colorR*/ = 1.0f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f, size_t /*numberOfCircleParts*/ = 30)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
 
         virtual VisualizationNodePtr createTorus(float /*radius*/, float /*tubeRadius*/, float /*completion*/ = 1.0f, float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f, float /*transparency*/ = 0.0f, int /*sides*/ = 8, int /*rings*/ = 30)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
 
         virtual VisualizationNodePtr createCircleArrow(float /*radius*/, float /*tubeRadius*/, float /*completion*/ = 1, float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f, float /*transparency*/ = 0.0f, int /*sides*/ = 8, int /*rings*/ = 30)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
 
         virtual VisualizationNodePtr createCylinder(float /*radius*/, float /*height*/, float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createCoordSystem(float /*scaling*/ = 1.0f, std::string* /*text*/ = NULL, float /*axisLength*/ = 100.0f, float /*axisSize*/ = 3.0f, int /*nrOfBlocks*/ = 10)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createBoundingBox(const BoundingBox& /*bbox*/, bool /*wireFrame*/ = false)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createVertexVisualization(const Eigen::Vector3f& /*position*/, float /*radius*/, float /*transparency*/,  float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
 
         virtual VisualizationNodePtr createTriMeshModelVisualization(TriMeshModelPtr /*model*/, Eigen::Matrix4f& /*pose*/, float /*scaleX*/ = 1.0f, float /*scaleY*/ = 1.0f, float /*scaleZ*/ = 1.0f)
         {
-	        return VisualizationNodePtr();
+            return nullptr;
         }
-        
+
         virtual VisualizationNodePtr createTriMeshModelVisualization(TriMeshModelPtr /*model*/, bool /*showNormals*/, Eigen::Matrix4f& /*pose*/, bool /*showLines*/ = true)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createPlane(const Eigen::Vector3f& /*position*/, const Eigen::Vector3f& /*normal*/, float /*extend*/, float /*transparency*/,  float /*colorR*/ = 0.5f, float /*colorG*/ = 0.5f, float /*colorB*/ = 0.5f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createPlane(const MathTools::Plane& plane, float extend, float transparency,  float colorR = 0.5f, float colorG = 0.5f, float colorB = 0.5f)
         {
@@ -190,15 +180,15 @@ namespace VirtualRobot
         }
         virtual VisualizationNodePtr createArrow(const Eigen::Vector3f& /*n*/, float /*length*/ = 50.0f, float /*width*/ = 2.0f, const Color& /*color*/ = Color::Gray())
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createTrajectory(TrajectoryPtr /*t*/, Color /*colorNode*/ = Color::Blue(), Color /*colorLine*/ = Color::Gray(), float /*nodeSize*/ = 15.0f, float /*lineSize*/ = 4.0f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         virtual VisualizationNodePtr createText(const std::string& /*text*/, bool /*billboard*/ = false, float /*scaling*/ = 1.0f, Color /*c*/ = Color::Black(), float /*offsetX*/ = 20.0f, float /*offsetY*/ = 20.0f, float /*offsetZ*/ = 0.0f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         /*!
             Creates an coordinate axis aligned ellipse
@@ -212,7 +202,7 @@ namespace VirtualRobot
         */
         virtual VisualizationNodePtr createEllipse(float /*x*/, float /*y*/, float /*z*/, bool /*showAxes*/ = true, float /*axesHeight*/ = 4.0f, float /*axesWidth*/ = 8.0f)
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
         /*!
             Move local visualization by homogeneous matrix m. (MM)
@@ -224,7 +214,7 @@ namespace VirtualRobot
         */
         virtual VisualizationNodePtr createVisualization()
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
 
         /*!
@@ -232,17 +222,14 @@ namespace VirtualRobot
         */
         virtual VisualizationNodePtr createUnitedVisualization(const std::vector<VisualizationNodePtr>& /*visualizations*/) const
         {
-            return VisualizationNodePtr();
+            return nullptr;
         }
 
         /*!
             Here, a manual cleanup can be called, no Coin3D access possible after this.
             Usually no need to call cleanup explicitly, since cleanup is performed automatically at application exit.
         */
-        virtual void cleanup()
-        {
-            ;
-        }
+        virtual void cleanup() {}
 
     };
     typedef boost::shared_ptr<VisualizationFactory::Color> ColorPtr;
diff --git a/VirtualRobot/examples/loadURDFRobot/loadURDFobot.cpp b/VirtualRobot/examples/loadURDFRobot/loadURDFobot.cpp
index 50948a405bd716bcde4bae886d1ec2cf6285f10b..ace7648eebd05d842c52284b8383e90a9a879f13 100644
--- a/VirtualRobot/examples/loadURDFRobot/loadURDFobot.cpp
+++ b/VirtualRobot/examples/loadURDFRobot/loadURDFobot.cpp
@@ -15,7 +15,7 @@ using std::endl;
 using namespace VirtualRobot;
 
 
-int main(int argc, char* argv[])
+int main(int /*argc*/, char* /*argv*/[])
 {
     SimoxURDFFactory f;