Ask Your Question
3

RGBDSLAM: Fixed for fuerte, where do I send the patch? [closed]

asked 2012-12-23 19:30:37 -0600

kk49 gravatar image

updated 2014-01-28 17:14:39 -0600

ngrennan gravatar image

I can't attached it here because of karma. So it is inlined below

Patch follows (possibly mangled)

Index: stack.xml
===================================================================
--- stack.xml   (revision 3770)
+++ stack.xml   (working copy)
@@ -9,6 +9,7 @@
   <depend stack="perception_pcl"/>
   <depend stack="vision_opencv"/>
   <depend stack="common_msgs"/>
+  <depend stack="libg2o">
   <!--depend stack="openni_kinect"/-->

 </stack>
Index: rgbdslam/src/graph_manager.h
===================================================================
--- rgbdslam/src/graph_manager.h    (revision 3770)
+++ rgbdslam/src/graph_manager.h    (working copy)
@@ -48,7 +48,7 @@
 #include <memory> //for auto_ptr
 #include "parameter_server.h"

-#include "g2o/core/graph_optimizer_sparse.h"
+#include "g2o/core/sparse_optimizer.h"

 #include "g2o/core/hyper_dijkstra.h"

Index: rgbdslam/src/edge.h
===================================================================
--- rgbdslam/src/edge.h (revision 3770)
+++ rgbdslam/src/edge.h (working copy)
@@ -19,7 +19,7 @@

 #include <set>
 #include <iostream>
-#include "g2o/math_groups/se3quat.h"
+#include "g2o/types/slam3d/se3quat.h"

 struct LoadedEdge3D
 {
Index: rgbdslam/src/graph_manager.cpp
===================================================================
--- rgbdslam/src/graph_manager.cpp  (revision 3770)
+++ rgbdslam/src/graph_manager.cpp  (working copy)
@@ -35,13 +35,14 @@
 #include <boost/foreach.hpp>
 #include <pcl_ros/point_cloud.h>

-#include "g2o/math_groups/se3quat.h"
-#include "g2o/types/slam3d/edge_se3_quat.h"
+#include "g2o/types/slam3d/se3quat.h"
+#include "g2o/types/slam3d/edge_se3.h"

 #include "g2o/core/block_solver.h"
 #include "g2o/solvers/csparse/linear_solver_csparse.h"
 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
 #include "g2o/solvers/pcg/linear_solver_pcg.h"
+#include "g2o/core/optimization_algorithm_dogleg.h"

 //typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> >  SlamBlockSolver;
 typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> >  SlamBlockSolver;
@@ -107,18 +108,18 @@
   if(backend == "cholmod" || backend == "auto"){
     SlamLinearCholmodSolver* linearSolver = new SlamLinearCholmodSolver();
     linearSolver->setBlockOrdering(false);
-    solver = new SlamBlockSolver(optimizer_, linearSolver);
+    solver = new SlamBlockSolver(linearSolver);
     current_backend_ = "cholmod";
   }
   else if(backend == "csparse"){
     SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
     linearSolver->setBlockOrdering(false);
-    solver = new SlamBlockSolver(optimizer_, linearSolver);
+    solver = new SlamBlockSolver(linearSolver);
     current_backend_ = "csparse";
   }
   else if(backend == "pcg"){
     SlamLinearPCGSolver* linearSolver = new SlamLinearPCGSolver();
-    solver = new SlamBlockSolver(optimizer_, linearSolver);
+    solver = new SlamBlockSolver(linearSolver);
     current_backend_ = "pcg";
   }
   else {
@@ -126,10 +127,12 @@
     ROS_INFO("Falling Back to Cholmod Solver");
     SlamLinearCholmodSolver* linearSolver = new SlamLinearCholmodSolver();
     linearSolver->setBlockOrdering(false);
-    solver = new SlamBlockSolver(optimizer_, linearSolver);
+    solver = new SlamBlockSolver(linearSolver);
     current_backend_ = "cholmod";
   }
-  optimizer_->setSolver(solver);
+
+  g2o::OptimizationAlgorithmDogleg * algo = new g2o::OptimizationAlgorithmDogleg(solver);
+  optimizer_->setAlgorithm(algo);
 }

 //WARNING: Dangerous
@@ -477,7 +480,7 @@
         //Send the current pose via tf nevertheless
         tf::Transform incremental = g2o2TF(mr.edge.mean);
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(prev_frame->id_));
-        tf::Transform previous = g2o2TF(v->estimate());
+        tf::Transform previous = g2o2TF(v->estimateAsSE3Quat());
         tf::Transform combined = previous*incremental;
         broadcastTransform(new_node, combined);
         process_node_runs_ = false;
@@ -734,10 +737,10 @@

                 Node* last = graph_.find(graph_.size()-1)->second;
                 marker_lines.points.push_back(
-                        pointInWorldFrame(last->feature_locations_3d_[newer_id], newer_v->estimate()));
+                        pointInWorldFrame(last->feature_locations_3d_[newer_id], newer_v->estimateAsSE3Quat()));
                 Node* prev = graph_.find(last_matching_node_)->second;
                 marker_lines.points.push_back(
-                        pointInWorldFrame(prev->feature_locations_3d_[earlier_id], earlier_v->estimate()));
+                        pointInWorldFrame(prev->feature_locations_3d_[earlier_id], earlier_v->estimateAsSE3Quat()));
             }
         }

@@ -755,10 +758,10 @@

             Node* last = graph_.find(graph_.size()-1)->second;
             marker_lines.points.push_back(
-                    pointInWorldFrame(last->feature_locations_3d_[newer_id], newer_v->estimate()));
+                    pointInWorldFrame(last->feature_locations_3d_[newer_id], newer_v->estimateAsSE3Quat()));
             Node* prev = graph_.find(last_matching_node_)->second;
             marker_lines.points.push_back(
-                    pointInWorldFrame(prev->feature_locations_3d_[earlier_id], earlier_v->estimate()));
+                    pointInWorldFrame(prev->feature_locations_3d_[earlier_id], earlier_v->estimateAsSE3Quat()));
         }

         ransac_marker_pub_.publish(marker_lines);
@@ -947,24 +950,25 @@
         v1->setId(edge.id1);
         v1->setEstimate(v2->estimate() * edge ...
(more)
edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by georgebrindeiro
close date 2013-01-16 02:22:54

Comments

Great, thank you! The answer "send it to Felix Fndres" below would be the correct one. I tested the patch and it is now applied to the public repository.

Felix Endres gravatar imageFelix Endres ( 2013-01-15 22:10:15 -0600 )edit

Since the patch was applied to trunk, I guess this can be closed as outdated.

georgebrindeiro gravatar imagegeorgebrindeiro ( 2013-01-16 02:23:34 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2013-01-08 07:19:03 -0600

updated 2013-01-16 02:21:49 -0600

Thanks a lot for the patch, it worked beautifully for me!

As mentioned in the wiki page for rgbdslam, patches should be sent by e-mail to the maintainer, Felix Endres. You can find his e-mail address here.

He is a forum moderator here, so maybe he would have seen it if you had tagged your answer. I am retagging the question for that purpose.

EDIT: I have added the patch as an attachment to the wiki page, as well as the instructions on how to apply the patch to the current working copy. The patch was applied to the trunk, no need for it anymore.

edit flag offensive delete link more

Comments

Perfect answer! Thanks for the wiki edit, I followed your instructions and it worked out. I applied the patch to the trunk now, so the patch doesn't need to be applied manually anymore. I'll therefore revert the wiki page.

Felix Endres gravatar imageFelix Endres ( 2013-01-15 22:13:40 -0600 )edit

Awesome, thanks!

georgebrindeiro gravatar imagegeorgebrindeiro ( 2013-01-16 02:16:58 -0600 )edit

Question Tools

Stats

Asked: 2012-12-23 19:30:37 -0600

Seen: 460 times

Last updated: Jan 16 '13