RGBDSLAM Fix for fuerte, where do I send the patch?
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.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)
Asked by kk49 on 2012-12-23 20:26:34 UTC
Comments