added a regularization penalty which will ask as mark_as_constant

Author: mpkuse

src/PoseGraphSLAM.cpp

@@ -1383,11 +1383,13 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
     reint_options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
     reint_options.minimizer_progress_to_stdout = false;
     reint_options.max_num_iterations = 50;
+    // reint_options.enable_fast_removal = true;
     eigenquaternion_parameterization = new ceres::EigenQuaternionParameterization;
     // robust_norm = new ceres::CauchyLoss(1.0);
     robust_norm = new ceres::HuberLoss(0.1);
 
     std::map< int , std::tuple<int,int> > changes_to_setid_on_set_union; // key:= worldID, value:= (prev setID of this world, new setID of this world).
+    vector<ceres::ResidualBlockId> regularization_terms_list;
 
 
     while( reinit_ceres_problem_onnewloopedge_optimize6DOF_isEnabled )
@@ -1855,19 +1857,43 @@ void PoseGraphSLAM::reinit_ceres_problem_onnewloopedge_optimize6DOF()
 
 
         //-----------------------------
-        //  Mark nodes as constant. Possibly also need to mark starts of each worlds as constants.
+        //  Mark nodes as constant. Possibly also need to mark starts of each worlds as constants. Can also try setting each sets 1st node as constant.
         //------------------------------
+        for( int v=0 ; v<regularization_terms_list.size() ; v++ ) {
+            reint_problem.RemoveResidualBlock( regularization_terms_list[v] );
+            cout << "Remove " << v << " th regularization term\n";
+        }
+        regularization_terms_list.clear();
+
         std::map< int, bool > mark_as_constant;
         // mark_as_constant[0] = true;
         mark_as_constant[1] = true;
         for( int ww=0 ; ww<manager->n_worlds(); ww++ ) {
-            if( mark_as_constant.count(ww) ==0  )
-                continue;
+            // if( mark_as_constant.count(ww) ==0  )
+                // continue;
+            int ww_setid = manager->getWorldsConstPtr()->find_setID_of_world_i(ww);
+            cout << "&&&&&&&&&&&&world#" << ww << " is in setID=" << ww_setid << endl;
+            if( ww_setid >= 0 && ww_setid==ww ) {
+
+
             int ww_start = manager->nodeidx_of_world_i_started( ww );
-            reint_problem.SetParameterBlockConstant(  get_raw_ptr_to_opt_variable_q(ww_start) );
-            reint_problem.SetParameterBlockConstant(  get_raw_ptr_to_opt_variable_t(ww_start)  );
-            cout << "Mark node#" << ww_start << " as constant\n";
+            // reint_problem.SetParameterBlockConstant(  get_raw_ptr_to_opt_variable_q(ww_start) );
+            // reint_problem.SetParameterBlockConstant(  get_raw_ptr_to_opt_variable_t(ww_start)  );
+
+            ceres::CostFunction * regularixa_cost = NodePoseRegularization::Create( getNodePose(ww_start), 1.2 );
+            ceres::ResidualBlockId resi_id = reint_problem.AddResidualBlock( regularixa_cost, NULL,  get_raw_ptr_to_opt_variable_q(ww_start), get_raw_ptr_to_opt_variable_t(ww_start) );
+            regularization_terms_list.push_back( resi_id );
+
+            cout << "node#" << ww_start << " is in world#" << ww << " which has setID as " << ww_setid << endl;
+            cout << "Mark node#" << ww_start << " as constant. \n";
+            }
+            else
+                cout << "skip\n";
         }
+        cout << "Total elements in `regularization_terms_list` = " << regularization_terms_list.size() << endl;
+
+
+
 
 
         //-----------------------

src/PoseGraphSLAM.h

@@ -293,26 +293,28 @@ class NodePoseRegularization
         npose.col(3).topRows(3) = p_1;
 
         // nodepose --> T
-        Matrix<T,4,4> f = nodepose.cast<float> ();
+        Matrix<T,4,4> f = nodepose.cast<T> ();
 
         Matrix<T,4,4> delta = f.inverse() * npose ;
-        Quaternion<T> delta_q( delta.topLeftCorner(3,3) );
+        Matrix<T,3,3> R = delta.topLeftCorner(3,3);
+        Quaternion<T> delta_q( R );
+
 
         Eigen::Map<Matrix<T,6,1> > residuals( residue_ptr );
         residuals.block(0,0, 3,1) = T(weight) * delta.col(3).topRows(3);
         residuals.block(3,0, 3,1) = T(weight) * T(2.0) * delta_q.vec();
-
+        
         return true;
     }
 
-    // static ceres::CostFunction* Create( const Matrix4d& _observed_node_pose, const double xweight )
-    // {
-    //   return ( new ceres::AutoDiffCostFunction<NodePoseRegularization,6,4,3>
-    //     (
-    //       new NodePoseRegularization(_observed_node_pose, xweight )
-    //     )
-    //   );
-    // }
+    static ceres::CostFunction* Create( const Matrix4d& _observed_node_pose, const double xweight )
+    {
+      return ( new ceres::AutoDiffCostFunction<NodePoseRegularization,6,4,3>
+        (
+          new NodePoseRegularization(_observed_node_pose, xweight )
+        )
+      );
+    }
 
 private:
     const Matrix4d& nodepose;