RGBDSLAM Fix for fuerte, where do I send the patch? [closed]
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 ...
Closed for the following reason
duplicate question by
Lorenz
close date 2012-12-23 21:45:49
add a comment