Added parameters to set solver max iterations and max threads

Author: fercabe

src/dllnode.hpp

@@ -66,6 +66,10 @@ class DLLNode
             m_initZOffset = 0.0;  
 		if(!lnh.getParam("align_method", m_alignMethod))
             m_alignMethod = 1;
+		if(!lnh.getParam("solver_max_iter", m_solverMaxIter))
+			m_solverMaxIter = 75;
+		if(!lnh.getParam("solver_max_threads", m_solverMaxThreads))
+			m_solverMaxThreads = 8;
 
 		// Init internal variables
 		m_init = false;
@@ -75,6 +79,10 @@ class DLLNode
 		// Compute trilinear interpolation map 
 		m_grid3d.computeTrilinearInterpolation();
 
+		// Setup solver parameters
+		m_solver.setMaxNumIterations(m_solverMaxIter);
+		m_solver.setMaxNumThreads(m_solverMaxThreads);
+
 		// Launch subscribers
 		m_pcSub = m_nh.subscribe(m_inCloudTopic, 1, &DLLNode::pointcloudCallback, this);
 		m_initialPoseSub = lnh.subscribe("initial_pose", 2, &DLLNode::initialPoseReceived, this);
@@ -397,7 +405,7 @@ class DLLNode
 	tf::Transform m_lastGlobalTf;
 	bool m_doUpdate;
 	double m_updateRate;
-	int m_alignMethod;
+	int m_alignMethod, m_solverMaxIter, m_solverMaxThreads;
 	ros::Time m_lastPeriodicUpdate;
 
 	//! Node parameters

src/dllsolver.hpp

@@ -90,13 +90,15 @@ class DLLSolver
 
     // Optimizer parameters
     int _max_num_iterations;
+    int _max_num_threads;
 
   public:
 
     DLLSolver(Grid3d &grid) : _grid(grid)
     {
         google::InitGoogleLogging("DLLSolver");
         _max_num_iterations = 100;
+        _max_num_threads = 8;
     }
 
     ~DLLSolver(void)
@@ -115,6 +117,17 @@ class DLLSolver
             return false;
     }
 
+    bool setMaxNumThreads(int n)
+    {
+        if(n>0)
+        {
+            _max_num_threads = n;
+            return true;
+        } 
+        else
+            return false;
+    }
+
     bool solve(std::vector<pcl::PointXYZ> &p, double &tx, double &ty, double &tz, double &yaw)
     {
         // Initial solution
@@ -136,7 +149,7 @@ class DLLSolver
         options.minimizer_progress_to_stdout = false;
         options.linear_solver_type = ceres::DENSE_QR;
         options.max_num_iterations = _max_num_iterations;
-        options.num_threads = 1; 
+        options.num_threads = _max_num_threads; 
         Solver::Summary summary;
         Solve(options, &problem, &summary);