diff --git a/corelib/include/rtabmap/core/OptimizerG2O.h b/corelib/include/rtabmap/core/OptimizerG2O.h
index 7de63bce..0f57b185 100644
--- a/corelib/include/rtabmap/core/OptimizerG2O.h
+++ b/corelib/include/rtabmap/core/OptimizerG2O.h
@@ -47,7 +47,7 @@ public:
 			bool useRobustConstraints = false);
 
 public:
-	OptimizerG2O(const ParametersMap & parameters) :
+	OptimizerG2O(const ParametersMap & parameters = ParametersMap()) :
 		Optimizer(parameters),
 		solver_(Parameters::defaultg2oSolver()),
 		optimizer_(Parameters::defaultg2oOptimizer())
diff --git a/corelib/src/util3d_motion_estimation.cpp b/corelib/src/util3d_motion_estimation.cpp
index c0f1cde4..9167602e 100644
--- a/corelib/src/util3d_motion_estimation.cpp
+++ b/corelib/src/util3d_motion_estimation.cpp
@@ -130,12 +130,6 @@ Transform estimateMotion3DTo2D(
 						   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
 						   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
 
-			UWARN("pnp=%s", pnp.prettyPrint().c_str());
-
-			OptimizerG2O g2o;
-			Transform g2oT = g2o.poseOptimization(pnp, objectPoints, imagePoints, cameraModel);
-			UWARN("g2oT=%s", g2oT.prettyPrint().c_str());
-
 			transform = (cameraModel.localTransform() * pnp).inverse();
 
 			// compute variance (like in PCL computeVariance() method of sac_model.h)