Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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

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(v1); 
-        latest_transform_ = g2o2QMatrix(v1->estimate()); 
+        latest_transform_ = g2o2QMatrix(v1->estimateAsSE3Quat()); 
     }
     else if (!v2 && v1) {
         v2 = new g2o::VertexSE3;
         assert(v2);
         v2->setId(edge.id2);
-        v2->setEstimate(v1->estimate() * edge.mean);
+        v2->setEstimate(v1->estimateAsSE3Quat() * edge.mean);
         optimizer_->addVertex(v2); 
-        latest_transform_ = g2o2QMatrix(v2->estimate()); 
+        latest_transform_ = g2o2QMatrix(v2->estimateAsSE3Quat()); 
     }
     else if(set_estimate){
-        v2->setEstimate(v1->estimate() * edge.mean);
+        v2->setEstimate(v1->estimateAsSE3Quat() * edge.mean);
     }
     g2o::EdgeSE3* g2o_edge = new g2o::EdgeSE3;
     g2o_edge->vertices()[0] = v1;
     g2o_edge->vertices()[1] = v2;
-    g2o_edge->setMeasurement(edge.mean);
-    g2o_edge->setInverseMeasurement(edge.mean.inverse());
+    //g2o_edge->setMeasurement(edge.mean);
+    g2o::SE3Quat edgeMean(edge.mean); //SHOULD BE FIXED in g2o
+    g2o_edge->setMeasurement(edgeMean);
     g2o_edge->setInformation(edge.informationMatrix);
     optimizer_->addEdge(g2o_edge);

@@ -1028,8 +1032,8 @@

   g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(optimizer_->vertices().size()-1));

-  computed_motion_ =  g2o2TF(v->estimate());
-  latest_transform_ = g2o2QMatrix(v->estimate()); 
+  computed_motion_ =  g2o2TF(v->estimateAsSE3Quat());
+  latest_transform_ = g2o2QMatrix(v->estimateAsSE3Quat()); 
   Q_EMIT setGraphEdges(getGraphEdges());
   Q_EMIT updateTransforms(getAllPosesAsMatrixList());

@@ -1126,7 +1130,7 @@
     for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
         if(v){ 
-            current_poses_.push_back(g2o2QMatrix(v->estimate())); 
+            current_poses_.push_back(g2o2QMatrix(v->estimateAsSE3Quat())); 
         } else {
             ROS_ERROR("Nullpointer in graph at position %i!", i);
         }
@@ -1183,7 +1187,7 @@
         cam2rgb.setOrigin(tf::Point(0,-0.04,0));
         world2base = cam2rgb*transform;
         */
-        tf::Transform pose = g2o2TF(v->estimate());
+        tf::Transform pose = g2o2TF(v->estimateAsSE3Quat());
         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         world2base = init_base_pose_*base2points*pose*base2points.inverse();

@@ -1248,7 +1252,7 @@
     int feat_count = 0;
     for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
-        tf::Transform world2cam = g2o2TF(v->estimate());
+        tf::Transform world2cam = g2o2TF(v->estimateAsSE3Quat());
         world2rgb = cam2rgb*world2cam;
         Eigen::Matrix4f world2rgbMat;
         pcl_ros::transformAsMatrix(world2rgb, world2rgbMat);
@@ -1299,7 +1303,7 @@
             ROS_ERROR("Nullpointer in graph at position %i!", i);
             continue;
         }
-        tf::Transform transform = g2o2TF(v->estimate());
+        tf::Transform transform = g2o2TF(v->estimateAsSE3Quat());
         world2cam = base2points*transform;
         transformAndAppendPointCloud (*(graph_[i]->pc_col), aggregate_cloud, world2cam, Max_Depth);

@@ -1397,7 +1401,7 @@
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
         ROS_ERROR_COND(!v, "Nullpointer in graph at position %i!", i);

-        tf::Transform pose = g2o2TF(v->estimate());
+        tf::Transform pose = g2o2TF(v->estimateAsSE3Quat());

         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         tf::Transform world2base = init_base_pose_*base2points*pose*base2points.inverse();
@@ -1474,7 +1478,7 @@

         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         printTransform("base2points", base2points);
-        tf::Transform computed_motion = g2o2TF(v->estimate());//get pose of point cloud w.r.t. first frame's pc
+        tf::Transform computed_motion = g2o2TF(v->estimateAsSE3Quat());//get pose of point cloud w.r.t. first frame's pc
         printTransform("computed_motion", computed_motion);
         printTransform("init_base_pose_", init_base_pose_);

Index: rgbdslam/src/misc.cpp
===================================================================
--- rgbdslam/src/misc.cpp   (revision 3770)
+++ rgbdslam/src/misc.cpp   (working copy)
@@ -23,7 +23,7 @@
 #include "parameter_server.h"
 #include <cv.h>

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

 #include <pcl_ros/transforms.h>

@@ -55,7 +55,7 @@

 QMatrix4x4 g2o2QMatrix(const g2o::SE3Quat se3) {
     struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
-    Eigen::Matrix<double, 4, 4> m = se3.to_homogenious_matrix(); //_Matrix< 4, 4, double >
+    Eigen::Matrix<double, 4, 4> m = se3.to_homogeneous_matrix(); //_Matrix< 4, 4, double >
     ROS_DEBUG_STREAM("Eigen Matrix:\n" << m);
     QMatrix4x4 qmat( static_cast<qreal*>( m.data() )  );
     // g2o/Eigen seems to use a different row-major/column-major array layout
Index: rgbdslam/launch/kinect+rgbdslam.launch
===================================================================
--- rgbdslam/launch/kinect+rgbdslam.launch  (revision 3770)
+++ rgbdslam/launch/kinect+rgbdslam.launch  (working copy)
@@ -1,5 +1,5 @@
 <launch>
-  <include file="$(find openni_camera)/launch/openni_node.launch"/>
+  <include file="$(find openni_launch)/launch/openni.launch"/>
   <arg name="debug" default="false"/>
   <!-- Several possibilities to debug (uncomment only one)-->
   <arg if="$(arg debug)" name="launch_prefix" value="/usr/bin/xterm -rv -e gdb -ex run -args"/>
Index: rgbdslam/CMakeLists.txt
===================================================================
--- rgbdslam/CMakeLists.txt (revision 3770)
+++ rgbdslam/CMakeLists.txt (working copy)
@@ -21,6 +21,8 @@
 cmake_minimum_required(VERSION 2.4.6)
 include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

+
+
 # Set the build type.  Options are:
 #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
 #  Debug          : w/ debug symbols, w/o optimization
@@ -54,7 +56,7 @@
 #rosbuild_link_boost(${PROJECT_NAME} thread)
 #rosbuild_add_executable(example examples/example.cpp)

-find_package(g2o REQUIRED)
+find_package(libg2o REQUIRED)
 include_directories(/usr/include/suitesparse)

 ##############################################################################
@@ -193,7 +195,8 @@
 rosbuild_add_executable(rgbdslam ${QT_SOURCES} ${QT_RESOURCES_CPP} ${QT_FORMS_HPP} ${QT_MOC_HPP})

 #set libs
-SET(G2O_LIBS cholmod g2o_core g2o_stuff g2o_types_slam3d g2o_solver_cholmod g2o_solver_pcg g2o_solver_csparse cxsparse g2o_incremental)
+#SET(G2O_LIBS cholmod g2o_core g2o_stuff g2o_types_slam3d g2o_solver_cholmod g2o_solver_pcg g2o_solver_csparse cxsparse g2o_incremental)
+SET(G2O_LIBS ${libg2o_LIBRARIES} cholmod cxsparse)
 #SET(LIBS_LINK GL GLU -lgl2ps ${G2O_LIBS} ${QT_LIBRARIES} ${QT_QTOPENGL_LIBRARY} ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${OpenCV_LIBS})
 SET(LIBS_LINK GL GLU ${G2O_LIBS} ${QT_LIBRARIES} ${QT_QTOPENGL_LIBRARY} ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${OpenCV_LIBS} -lboost_signals -lrt)
click to hide/show revision 2
retagged

updated 2012-12-24 03:57:28 -0600

joq gravatar image

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

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(v1); 
-        latest_transform_ = g2o2QMatrix(v1->estimate()); 
+        latest_transform_ = g2o2QMatrix(v1->estimateAsSE3Quat()); 
     }
     else if (!v2 && v1) {
         v2 = new g2o::VertexSE3;
         assert(v2);
         v2->setId(edge.id2);
-        v2->setEstimate(v1->estimate() * edge.mean);
+        v2->setEstimate(v1->estimateAsSE3Quat() * edge.mean);
         optimizer_->addVertex(v2); 
-        latest_transform_ = g2o2QMatrix(v2->estimate()); 
+        latest_transform_ = g2o2QMatrix(v2->estimateAsSE3Quat()); 
     }
     else if(set_estimate){
-        v2->setEstimate(v1->estimate() * edge.mean);
+        v2->setEstimate(v1->estimateAsSE3Quat() * edge.mean);
     }
     g2o::EdgeSE3* g2o_edge = new g2o::EdgeSE3;
     g2o_edge->vertices()[0] = v1;
     g2o_edge->vertices()[1] = v2;
-    g2o_edge->setMeasurement(edge.mean);
-    g2o_edge->setInverseMeasurement(edge.mean.inverse());
+    //g2o_edge->setMeasurement(edge.mean);
+    g2o::SE3Quat edgeMean(edge.mean); //SHOULD BE FIXED in g2o
+    g2o_edge->setMeasurement(edgeMean);
     g2o_edge->setInformation(edge.informationMatrix);
     optimizer_->addEdge(g2o_edge);

@@ -1028,8 +1032,8 @@

   g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(optimizer_->vertices().size()-1));

-  computed_motion_ =  g2o2TF(v->estimate());
-  latest_transform_ = g2o2QMatrix(v->estimate()); 
+  computed_motion_ =  g2o2TF(v->estimateAsSE3Quat());
+  latest_transform_ = g2o2QMatrix(v->estimateAsSE3Quat()); 
   Q_EMIT setGraphEdges(getGraphEdges());
   Q_EMIT updateTransforms(getAllPosesAsMatrixList());

@@ -1126,7 +1130,7 @@
     for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
         if(v){ 
-            current_poses_.push_back(g2o2QMatrix(v->estimate())); 
+            current_poses_.push_back(g2o2QMatrix(v->estimateAsSE3Quat())); 
         } else {
             ROS_ERROR("Nullpointer in graph at position %i!", i);
         }
@@ -1183,7 +1187,7 @@
         cam2rgb.setOrigin(tf::Point(0,-0.04,0));
         world2base = cam2rgb*transform;
         */
-        tf::Transform pose = g2o2TF(v->estimate());
+        tf::Transform pose = g2o2TF(v->estimateAsSE3Quat());
         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         world2base = init_base_pose_*base2points*pose*base2points.inverse();

@@ -1248,7 +1252,7 @@
     int feat_count = 0;
     for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
-        tf::Transform world2cam = g2o2TF(v->estimate());
+        tf::Transform world2cam = g2o2TF(v->estimateAsSE3Quat());
         world2rgb = cam2rgb*world2cam;
         Eigen::Matrix4f world2rgbMat;
         pcl_ros::transformAsMatrix(world2rgb, world2rgbMat);
@@ -1299,7 +1303,7 @@
             ROS_ERROR("Nullpointer in graph at position %i!", i);
             continue;
         }
-        tf::Transform transform = g2o2TF(v->estimate());
+        tf::Transform transform = g2o2TF(v->estimateAsSE3Quat());
         world2cam = base2points*transform;
         transformAndAppendPointCloud (*(graph_[i]->pc_col), aggregate_cloud, world2cam, Max_Depth);

@@ -1397,7 +1401,7 @@
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
         ROS_ERROR_COND(!v, "Nullpointer in graph at position %i!", i);

-        tf::Transform pose = g2o2TF(v->estimate());
+        tf::Transform pose = g2o2TF(v->estimateAsSE3Quat());

         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         tf::Transform world2base = init_base_pose_*base2points*pose*base2points.inverse();
@@ -1474,7 +1478,7 @@

         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         printTransform("base2points", base2points);
-        tf::Transform computed_motion = g2o2TF(v->estimate());//get pose of point cloud w.r.t. first frame's pc
+        tf::Transform computed_motion = g2o2TF(v->estimateAsSE3Quat());//get pose of point cloud w.r.t. first frame's pc
         printTransform("computed_motion", computed_motion);
         printTransform("init_base_pose_", init_base_pose_);

Index: rgbdslam/src/misc.cpp
===================================================================
--- rgbdslam/src/misc.cpp   (revision 3770)
+++ rgbdslam/src/misc.cpp   (working copy)
@@ -23,7 +23,7 @@
 #include "parameter_server.h"
 #include <cv.h>

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

 #include <pcl_ros/transforms.h>

@@ -55,7 +55,7 @@

 QMatrix4x4 g2o2QMatrix(const g2o::SE3Quat se3) {
     struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
-    Eigen::Matrix<double, 4, 4> m = se3.to_homogenious_matrix(); //_Matrix< 4, 4, double >
+    Eigen::Matrix<double, 4, 4> m = se3.to_homogeneous_matrix(); //_Matrix< 4, 4, double >
     ROS_DEBUG_STREAM("Eigen Matrix:\n" << m);
     QMatrix4x4 qmat( static_cast<qreal*>( m.data() )  );
     // g2o/Eigen seems to use a different row-major/column-major array layout
Index: rgbdslam/launch/kinect+rgbdslam.launch
===================================================================
--- rgbdslam/launch/kinect+rgbdslam.launch  (revision 3770)
+++ rgbdslam/launch/kinect+rgbdslam.launch  (working copy)
@@ -1,5 +1,5 @@
 <launch>
-  <include file="$(find openni_camera)/launch/openni_node.launch"/>
+  <include file="$(find openni_launch)/launch/openni.launch"/>
   <arg name="debug" default="false"/>
   <!-- Several possibilities to debug (uncomment only one)-->
   <arg if="$(arg debug)" name="launch_prefix" value="/usr/bin/xterm -rv -e gdb -ex run -args"/>
Index: rgbdslam/CMakeLists.txt
===================================================================
--- rgbdslam/CMakeLists.txt (revision 3770)
+++ rgbdslam/CMakeLists.txt (working copy)
@@ -21,6 +21,8 @@
 cmake_minimum_required(VERSION 2.4.6)
 include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

+
+
 # Set the build type.  Options are:
 #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
 #  Debug          : w/ debug symbols, w/o optimization
@@ -54,7 +56,7 @@
 #rosbuild_link_boost(${PROJECT_NAME} thread)
 #rosbuild_add_executable(example examples/example.cpp)

-find_package(g2o REQUIRED)
+find_package(libg2o REQUIRED)
 include_directories(/usr/include/suitesparse)

 ##############################################################################
@@ -193,7 +195,8 @@
 rosbuild_add_executable(rgbdslam ${QT_SOURCES} ${QT_RESOURCES_CPP} ${QT_FORMS_HPP} ${QT_MOC_HPP})

 #set libs
-SET(G2O_LIBS cholmod g2o_core g2o_stuff g2o_types_slam3d g2o_solver_cholmod g2o_solver_pcg g2o_solver_csparse cxsparse g2o_incremental)
+#SET(G2O_LIBS cholmod g2o_core g2o_stuff g2o_types_slam3d g2o_solver_cholmod g2o_solver_pcg g2o_solver_csparse cxsparse g2o_incremental)
+SET(G2O_LIBS ${libg2o_LIBRARIES} cholmod cxsparse)
 #SET(LIBS_LINK GL GLU -lgl2ps ${G2O_LIBS} ${QT_LIBRARIES} ${QT_QTOPENGL_LIBRARY} ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${OpenCV_LIBS})
 SET(LIBS_LINK GL GLU ${G2O_LIBS} ${QT_LIBRARIES} ${QT_QTOPENGL_LIBRARY} ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${OpenCV_LIBS} -lboost_signals -lrt)

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

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(v1); 
-        latest_transform_ = g2o2QMatrix(v1->estimate()); 
+        latest_transform_ = g2o2QMatrix(v1->estimateAsSE3Quat()); 
     }
     else if (!v2 && v1) {
         v2 = new g2o::VertexSE3;
         assert(v2);
         v2->setId(edge.id2);
-        v2->setEstimate(v1->estimate() * edge.mean);
+        v2->setEstimate(v1->estimateAsSE3Quat() * edge.mean);
         optimizer_->addVertex(v2); 
-        latest_transform_ = g2o2QMatrix(v2->estimate()); 
+        latest_transform_ = g2o2QMatrix(v2->estimateAsSE3Quat()); 
     }
     else if(set_estimate){
-        v2->setEstimate(v1->estimate() * edge.mean);
+        v2->setEstimate(v1->estimateAsSE3Quat() * edge.mean);
     }
     g2o::EdgeSE3* g2o_edge = new g2o::EdgeSE3;
     g2o_edge->vertices()[0] = v1;
     g2o_edge->vertices()[1] = v2;
-    g2o_edge->setMeasurement(edge.mean);
-    g2o_edge->setInverseMeasurement(edge.mean.inverse());
+    //g2o_edge->setMeasurement(edge.mean);
+    g2o::SE3Quat edgeMean(edge.mean); //SHOULD BE FIXED in g2o
+    g2o_edge->setMeasurement(edgeMean);
     g2o_edge->setInformation(edge.informationMatrix);
     optimizer_->addEdge(g2o_edge);

@@ -1028,8 +1032,8 @@

   g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(optimizer_->vertices().size()-1));

-  computed_motion_ =  g2o2TF(v->estimate());
-  latest_transform_ = g2o2QMatrix(v->estimate()); 
+  computed_motion_ =  g2o2TF(v->estimateAsSE3Quat());
+  latest_transform_ = g2o2QMatrix(v->estimateAsSE3Quat()); 
   Q_EMIT setGraphEdges(getGraphEdges());
   Q_EMIT updateTransforms(getAllPosesAsMatrixList());

@@ -1126,7 +1130,7 @@
     for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
         if(v){ 
-            current_poses_.push_back(g2o2QMatrix(v->estimate())); 
+            current_poses_.push_back(g2o2QMatrix(v->estimateAsSE3Quat())); 
         } else {
             ROS_ERROR("Nullpointer in graph at position %i!", i);
         }
@@ -1183,7 +1187,7 @@
         cam2rgb.setOrigin(tf::Point(0,-0.04,0));
         world2base = cam2rgb*transform;
         */
-        tf::Transform pose = g2o2TF(v->estimate());
+        tf::Transform pose = g2o2TF(v->estimateAsSE3Quat());
         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         world2base = init_base_pose_*base2points*pose*base2points.inverse();

@@ -1248,7 +1252,7 @@
     int feat_count = 0;
     for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
-        tf::Transform world2cam = g2o2TF(v->estimate());
+        tf::Transform world2cam = g2o2TF(v->estimateAsSE3Quat());
         world2rgb = cam2rgb*world2cam;
         Eigen::Matrix4f world2rgbMat;
         pcl_ros::transformAsMatrix(world2rgb, world2rgbMat);
@@ -1299,7 +1303,7 @@
             ROS_ERROR("Nullpointer in graph at position %i!", i);
             continue;
         }
-        tf::Transform transform = g2o2TF(v->estimate());
+        tf::Transform transform = g2o2TF(v->estimateAsSE3Quat());
         world2cam = base2points*transform;
         transformAndAppendPointCloud (*(graph_[i]->pc_col), aggregate_cloud, world2cam, Max_Depth);

@@ -1397,7 +1401,7 @@
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
         ROS_ERROR_COND(!v, "Nullpointer in graph at position %i!", i);

-        tf::Transform pose = g2o2TF(v->estimate());
+        tf::Transform pose = g2o2TF(v->estimateAsSE3Quat());

         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         tf::Transform world2base = init_base_pose_*base2points*pose*base2points.inverse();
@@ -1474,7 +1478,7 @@

         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         printTransform("base2points", base2points);
-        tf::Transform computed_motion = g2o2TF(v->estimate());//get pose of point cloud w.r.t. first frame's pc
+        tf::Transform computed_motion = g2o2TF(v->estimateAsSE3Quat());//get pose of point cloud w.r.t. first frame's pc
         printTransform("computed_motion", computed_motion);
         printTransform("init_base_pose_", init_base_pose_);

Index: rgbdslam/src/misc.cpp
===================================================================
--- rgbdslam/src/misc.cpp   (revision 3770)
+++ rgbdslam/src/misc.cpp   (working copy)
@@ -23,7 +23,7 @@
 #include "parameter_server.h"
 #include <cv.h>

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

 #include <pcl_ros/transforms.h>

@@ -55,7 +55,7 @@

 QMatrix4x4 g2o2QMatrix(const g2o::SE3Quat se3) {
     struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
-    Eigen::Matrix<double, 4, 4> m = se3.to_homogenious_matrix(); //_Matrix< 4, 4, double >
+    Eigen::Matrix<double, 4, 4> m = se3.to_homogeneous_matrix(); //_Matrix< 4, 4, double >
     ROS_DEBUG_STREAM("Eigen Matrix:\n" << m);
     QMatrix4x4 qmat( static_cast<qreal*>( m.data() )  );
     // g2o/Eigen seems to use a different row-major/column-major array layout
Index: rgbdslam/launch/kinect+rgbdslam.launch
===================================================================
--- rgbdslam/launch/kinect+rgbdslam.launch  (revision 3770)
+++ rgbdslam/launch/kinect+rgbdslam.launch  (working copy)
@@ -1,5 +1,5 @@
 <launch>
-  <include file="$(find openni_camera)/launch/openni_node.launch"/>
+  <include file="$(find openni_launch)/launch/openni.launch"/>
   <arg name="debug" default="false"/>
   <!-- Several possibilities to debug (uncomment only one)-->
   <arg if="$(arg debug)" name="launch_prefix" value="/usr/bin/xterm -rv -e gdb -ex run -args"/>
Index: rgbdslam/CMakeLists.txt
===================================================================
--- rgbdslam/CMakeLists.txt (revision 3770)
+++ rgbdslam/CMakeLists.txt (working copy)
@@ -21,6 +21,8 @@
 cmake_minimum_required(VERSION 2.4.6)
 include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

+
+
 # Set the build type.  Options are:
 #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
 #  Debug          : w/ debug symbols, w/o optimization
@@ -54,7 +56,7 @@
 #rosbuild_link_boost(${PROJECT_NAME} thread)
 #rosbuild_add_executable(example examples/example.cpp)

-find_package(g2o REQUIRED)
+find_package(libg2o REQUIRED)
 include_directories(/usr/include/suitesparse)

 ##############################################################################
@@ -193,7 +195,8 @@
 rosbuild_add_executable(rgbdslam ${QT_SOURCES} ${QT_RESOURCES_CPP} ${QT_FORMS_HPP} ${QT_MOC_HPP})

 #set libs
-SET(G2O_LIBS cholmod g2o_core g2o_stuff g2o_types_slam3d g2o_solver_cholmod g2o_solver_pcg g2o_solver_csparse cxsparse g2o_incremental)
+#SET(G2O_LIBS cholmod g2o_core g2o_stuff g2o_types_slam3d g2o_solver_cholmod g2o_solver_pcg g2o_solver_csparse cxsparse g2o_incremental)
+SET(G2O_LIBS ${libg2o_LIBRARIES} cholmod cxsparse)
 #SET(LIBS_LINK GL GLU -lgl2ps ${G2O_LIBS} ${QT_LIBRARIES} ${QT_QTOPENGL_LIBRARY} ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${OpenCV_LIBS})
 SET(LIBS_LINK GL GLU ${G2O_LIBS} ${QT_LIBRARIES} ${QT_QTOPENGL_LIBRARY} ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${OpenCV_LIBS} -lboost_signals -lrt)

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

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(v1); 
-        latest_transform_ = g2o2QMatrix(v1->estimate()); 
+        latest_transform_ = g2o2QMatrix(v1->estimateAsSE3Quat()); 
     }
     else if (!v2 && v1) {
         v2 = new g2o::VertexSE3;
         assert(v2);
         v2->setId(edge.id2);
-        v2->setEstimate(v1->estimate() * edge.mean);
+        v2->setEstimate(v1->estimateAsSE3Quat() * edge.mean);
         optimizer_->addVertex(v2); 
-        latest_transform_ = g2o2QMatrix(v2->estimate()); 
+        latest_transform_ = g2o2QMatrix(v2->estimateAsSE3Quat()); 
     }
     else if(set_estimate){
-        v2->setEstimate(v1->estimate() * edge.mean);
+        v2->setEstimate(v1->estimateAsSE3Quat() * edge.mean);
     }
     g2o::EdgeSE3* g2o_edge = new g2o::EdgeSE3;
     g2o_edge->vertices()[0] = v1;
     g2o_edge->vertices()[1] = v2;
-    g2o_edge->setMeasurement(edge.mean);
-    g2o_edge->setInverseMeasurement(edge.mean.inverse());
+    //g2o_edge->setMeasurement(edge.mean);
+    g2o::SE3Quat edgeMean(edge.mean); //SHOULD BE FIXED in g2o
+    g2o_edge->setMeasurement(edgeMean);
     g2o_edge->setInformation(edge.informationMatrix);
     optimizer_->addEdge(g2o_edge);

@@ -1028,8 +1032,8 @@

   g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(optimizer_->vertices().size()-1));

-  computed_motion_ =  g2o2TF(v->estimate());
-  latest_transform_ = g2o2QMatrix(v->estimate()); 
+  computed_motion_ =  g2o2TF(v->estimateAsSE3Quat());
+  latest_transform_ = g2o2QMatrix(v->estimateAsSE3Quat()); 
   Q_EMIT setGraphEdges(getGraphEdges());
   Q_EMIT updateTransforms(getAllPosesAsMatrixList());

@@ -1126,7 +1130,7 @@
     for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
         if(v){ 
-            current_poses_.push_back(g2o2QMatrix(v->estimate())); 
+            current_poses_.push_back(g2o2QMatrix(v->estimateAsSE3Quat())); 
         } else {
             ROS_ERROR("Nullpointer in graph at position %i!", i);
         }
@@ -1183,7 +1187,7 @@
         cam2rgb.setOrigin(tf::Point(0,-0.04,0));
         world2base = cam2rgb*transform;
         */
-        tf::Transform pose = g2o2TF(v->estimate());
+        tf::Transform pose = g2o2TF(v->estimateAsSE3Quat());
         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         world2base = init_base_pose_*base2points*pose*base2points.inverse();

@@ -1248,7 +1252,7 @@
     int feat_count = 0;
     for (unsigned int i = 0; i < optimizer_->vertices().size(); ++i) {
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
-        tf::Transform world2cam = g2o2TF(v->estimate());
+        tf::Transform world2cam = g2o2TF(v->estimateAsSE3Quat());
         world2rgb = cam2rgb*world2cam;
         Eigen::Matrix4f world2rgbMat;
         pcl_ros::transformAsMatrix(world2rgb, world2rgbMat);
@@ -1299,7 +1303,7 @@
             ROS_ERROR("Nullpointer in graph at position %i!", i);
             continue;
         }
-        tf::Transform transform = g2o2TF(v->estimate());
+        tf::Transform transform = g2o2TF(v->estimateAsSE3Quat());
         world2cam = base2points*transform;
         transformAndAppendPointCloud (*(graph_[i]->pc_col), aggregate_cloud, world2cam, Max_Depth);

@@ -1397,7 +1401,7 @@
         g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(optimizer_->vertex(i));
         ROS_ERROR_COND(!v, "Nullpointer in graph at position %i!", i);

-        tf::Transform pose = g2o2TF(v->estimate());
+        tf::Transform pose = g2o2TF(v->estimateAsSE3Quat());

         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         tf::Transform world2base = init_base_pose_*base2points*pose*base2points.inverse();
@@ -1474,7 +1478,7 @@

         tf::StampedTransform base2points = graph_[i]->getBase2PointsTransform();//get pose of base w.r.t current pc at capture time
         printTransform("base2points", base2points);
-        tf::Transform computed_motion = g2o2TF(v->estimate());//get pose of point cloud w.r.t. first frame's pc
+        tf::Transform computed_motion = g2o2TF(v->estimateAsSE3Quat());//get pose of point cloud w.r.t. first frame's pc
         printTransform("computed_motion", computed_motion);
         printTransform("init_base_pose_", init_base_pose_);

Index: rgbdslam/src/misc.cpp
===================================================================
--- rgbdslam/src/misc.cpp   (revision 3770)
+++ rgbdslam/src/misc.cpp   (working copy)
@@ -23,7 +23,7 @@
 #include "parameter_server.h"
 #include <cv.h>

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

 #include <pcl_ros/transforms.h>

@@ -55,7 +55,7 @@

 QMatrix4x4 g2o2QMatrix(const g2o::SE3Quat se3) {
     struct timespec starttime, finish; double elapsed; clock_gettime(CLOCK_MONOTONIC, &starttime);
-    Eigen::Matrix<double, 4, 4> m = se3.to_homogenious_matrix(); //_Matrix< 4, 4, double >
+    Eigen::Matrix<double, 4, 4> m = se3.to_homogeneous_matrix(); //_Matrix< 4, 4, double >
     ROS_DEBUG_STREAM("Eigen Matrix:\n" << m);
     QMatrix4x4 qmat( static_cast<qreal*>( m.data() )  );
     // g2o/Eigen seems to use a different row-major/column-major array layout
Index: rgbdslam/launch/kinect+rgbdslam.launch
===================================================================
--- rgbdslam/launch/kinect+rgbdslam.launch  (revision 3770)
+++ rgbdslam/launch/kinect+rgbdslam.launch  (working copy)
@@ -1,5 +1,5 @@
 <launch>
-  <include file="$(find openni_camera)/launch/openni_node.launch"/>
+  <include file="$(find openni_launch)/launch/openni.launch"/>
   <arg name="debug" default="false"/>
   <!-- Several possibilities to debug (uncomment only one)-->
   <arg if="$(arg debug)" name="launch_prefix" value="/usr/bin/xterm -rv -e gdb -ex run -args"/>
Index: rgbdslam/CMakeLists.txt
===================================================================
--- rgbdslam/CMakeLists.txt (revision 3770)
+++ rgbdslam/CMakeLists.txt (working copy)
@@ -21,6 +21,8 @@
 cmake_minimum_required(VERSION 2.4.6)
 include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

+
+
 # Set the build type.  Options are:
 #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
 #  Debug          : w/ debug symbols, w/o optimization
@@ -54,7 +56,7 @@
 #rosbuild_link_boost(${PROJECT_NAME} thread)
 #rosbuild_add_executable(example examples/example.cpp)

-find_package(g2o REQUIRED)
+find_package(libg2o REQUIRED)
 include_directories(/usr/include/suitesparse)

 ##############################################################################
@@ -193,7 +195,8 @@
 rosbuild_add_executable(rgbdslam ${QT_SOURCES} ${QT_RESOURCES_CPP} ${QT_FORMS_HPP} ${QT_MOC_HPP})

 #set libs
-SET(G2O_LIBS cholmod g2o_core g2o_stuff g2o_types_slam3d g2o_solver_cholmod g2o_solver_pcg g2o_solver_csparse cxsparse g2o_incremental)
+#SET(G2O_LIBS cholmod g2o_core g2o_stuff g2o_types_slam3d g2o_solver_cholmod g2o_solver_pcg g2o_solver_csparse cxsparse g2o_incremental)
+SET(G2O_LIBS ${libg2o_LIBRARIES} cholmod cxsparse)
 #SET(LIBS_LINK GL GLU -lgl2ps ${G2O_LIBS} ${QT_LIBRARIES} ${QT_QTOPENGL_LIBRARY} ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${OpenCV_LIBS})
 SET(LIBS_LINK GL GLU ${G2O_LIBS} ${QT_LIBRARIES} ${QT_QTOPENGL_LIBRARY} ${GLUT_LIBRARY} ${OPENGL_LIBRARY} ${OpenCV_LIBS} -lboost_signals -lrt)