ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

RGBDSLAM who do I send the patches too? (Fixed for latest fuerte)

asked 2012-12-23 19:24:13 -0500

kk49 gravatar image

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

ngrennan gravatar image

I can't even attached here because of karma. Ugh.

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.mean.inverse());
         optimizer_->addVertex ...
(more)
edit retag flag offensive close merge delete

Comments

Why you need to post a question four times?

Po-Jen Lai gravatar image Po-Jen Lai  ( 2012-12-23 19:54:52 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2012-12-23 21:47:01 -0500

Lorenz gravatar image

updated 2012-12-23 23:20:58 -0500

rgbdslam is hosted on Freiburg's googlecode page so I would file a ticket with the patch attached there. https://code.google.com/p/alufr-ros-pkg/

edit flag offensive delete link more

Question Tools

Stats

Asked: 2012-12-23 19:24:13 -0500

Seen: 198 times

Last updated: Dec 23 '12