2013-12-15

I am editing my problem and leaving previous versions- I know this post looks very large now but if somebody will have same problem in futer maby he can track down some of my mistakes and use them.

I'm using
ubuntu 12.04
ROS hydro medusa

From several days I'm trying to use some how rgbdslam but I get several errors.
I tried to do what is written here:
http://answers.ros.org/question/91111/rgbdslam-in-ros-hydro/

but after executing:
rosdep install rgbdslam_freiburg

I receive this error:

ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
rgbdslam: Missing resource pcl

ROS path [0]=/opt/ros/hydro/share/ros

ROS path [1]=/home/wilk/catkin_ws/src

ROS path [2]=/opt/ros/hydro/share

ROS path [3]=/opt/ros/hydro/stacks

Does it means that there is no pcl library provided with my ros installation or that I should made conversions from older pcl library as it worked for somebody here:
http://answers.ros.org/question/74899/rosmake-rgbdslam_freiburg-error-in-groovy/

I read also this but I don't know if I should also make it:
http://www.pcl-users.org/How-do-I-use-PCL-from-ROS-Hydro-td4029613.html

Could any one be as kind to tell me what I should do? or even which commands should I use in terminal. I would also d'like to know if making those conversations will solve the problem, if there is any one who can share it. I really don't like the idea of manually changing code without knowing if it will help.

Edit1:
Is it possible to use rosbuild and catkin workspaces simultaniously? Despite trying to use code writen for older version of ROS I'm trying to convert it too catkin_ws too. There are only a few files that need to be changed so I will pase them here. Please help.

I receave fallowing error:

CMake Error at rgbdslam_freiburg/rgbdslam/CMakeLists.txt:240 (add_executable):
add_executable cannot create target "rgbdslam" because another target with
the same name already exists. The existing target is a shared library
created in source directory
"/home/wilk/catkin_ws/src/rgbdslam_freiburg/rgbdslam". See documentation
for policy CMP0002 for more details.

My files looks like that:
CMakeLists.txt

# Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html
# Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html cmake_minimum_required(VERSION 2.8.3) project(rgbdslam)
# Load catkin and all dependencies required for this package
# TODO: remove all from COMPONENTS that are not catkin packages. find_package(catkin REQUIRED COMPONENTS tf pcl_conversions rospy roscpp rosbag cv_bridge sensor_msgs octomap_server octomap_ros geometry_msgs visualization_msgs eigen)

# include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})#########################################################
### CONFIG ##############################################
#########################################################
# 1, if SiftGPU should be used. For additional information
# see README set(USE_SIFT_GPU 0) set(ENV{SIFT_GPU_MODE} 2) #CUDA = 1, GLSL = 2 set(ENV{SIFT_GPU_CUDA_PATH} /usr/local/cuda) set(USE_GICP_BIN 0) set(USE_GICP_CODE 0) set(USE_PCL_ICP 1) set(USE_GL2PS 0) #For printing the 3D Window to PostScript - requires installation of additional library: libgl2ps-dev IF ("${ROS_PARALLEL_JOBS}" EQUAL "") set(ROS_PARALLEL_JOBS 1) endif ("${ROS_PARALLEL_JOBS}" EQUAL "")
#########################################################
#########################################################
#########################################################

set(ROS_COMPILE_FLAGS ${ROS_COMPILE_FLAGS} -fopenmp)

# CATKIN_MIGRATION: removed during catkin migration
# cmake_minimum_required(VERSION 2.4.6)

# CATKIN_MIGRATION: removed during catkin migration
# 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
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries set(ROS_BUILD_TYPE RelWithDebInfo)
#add_definitions(-DROSCONSOLE_SEVERITY_INFO) IF (${USE_GL2PS}) add_definitions(-DGL2PS) ENDIF (${USE_GL2PS}) set(CMAKE_CXX_FLAGS "-O3")
#is used for compiling the libraries set(USE_GICP 0) IF (${USE_GICP_BIN} OR ${USE_GICP_CODE}) set(USE_GICP 1) ENDIF (${USE_GICP_BIN} OR ${USE_GICP_CODE})

# CATKIN_MIGRATION: removed during catkin migration
# rosbuild_init()

#set the default path for built executables to the "bin" directory set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

#uncomment if you have defined messages
#add_message_files(
# FILES # TODO: List your msg files here
# )
#uncomment if you have defined services
#add_service_files(
# FILES # TODO: List your msg files here
#)
#common commands for building c++ executables and libraries
#add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#
# CATKIN_MIGRATION: removed during catkin migration
# rosbuild_add_boost_directories()
#find_package(Boost REQUIRED COMPONENTS thread) add_library(${PROJECT_NAME} ${Boost_INCLUDE_DIRS})
# include_directories(${Boost_INCLUDE_DIRS}) target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES})
#add_executable(example examples/example.cpp)

##############################################################################
# Required 3rd party
##############################################################################

#############################
# Octomap ###################
############################# find_package(octomap REQUIRED) include_directories(${OCTOMAP_INCLUDE_DIRS}) link_directories(${OCTOMAP_LIBRARY_DIRS}) link_libraries(${OCTOMAP_LIBRARIES})

#############################
# Qt ########################
#############################
# http://qtnode.net/wiki/Qt4_with_cmake find_package(Qt4 REQUIRED) SET(QT_USE_QTXML TRUE) SET(QT_USE_QTOPENGL TRUE) ADD_DEFINITIONS(-DQT_NO_KEYWORDS) include(${QT_USE_FILE})

# This is necessary as all ui files etc will get dumped in the bottom of then binary directory. include_directories(${CMAKE_CURRENT_BINARY_DIR} ${QT_QTOPENGL_INCLUDE_DIR} external/gicp/ann_1.1.2/include/ANN)

#get_directory_property(clean ADDITIONAL_MAKE_CLEAN_FILES)
#SET(clean external/siftgpu/linux/bin/libsiftgpu.so)
#set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${clean}") LINK_DIRECTORIES(${LIBRARY_OUTPUT_PATH}) SET(${CMAKE_LIBRARY_PATH} ${LIBRARY_OUTPUT_PATH})
#SET(CMAKE_INCLUDE_DIRECTORIES_PROJECT_BEFORE TRUE)

ADD_DEFINITIONS(-DDO_FEATURE_OPTIMIZATION)

#Only possible when not subscribing to point cloud topic. Possibly dangerous anyway, but halves storage space per point cloud
#ADD_DEFINITIONS(-DRGB_IS_4TH_DIM)

#############################
# SiftGPU ###################
############################# IF (${USE_SIFT_GPU}) SET(sift_gpu_build_path ${CMAKE_SOURCE_DIR}/external/siftgpu/linux)

include_directories(${CMAKE_CURRENT_BINARY_DIR} external/siftgpu/src) MESSAGE("\n------------------------------------------------------------------\n") MESSAGE("Compiling SiftGPU...") EXECUTE_PROCESS( COMMAND make ${ROS_PARALLEL_JOBS} siftgpu WORKING_DIRECTORY ${sift_gpu_build_path} RESULT_VARIABLE sift_gpu_return ) MESSAGE("\n------------------------------------------------------------------\n") #on error IF (NOT ${sift_gpu_return} EQUAL 0) MESSAGE(FATAL_ERROR "SiftGPU cannot be compiled. Returned: ${sift_gpu_return}") ENDIF (NOT ${sift_gpu_return} EQUAL 0) FILE(COPY external/siftgpu/linux/bin/libsiftgpu.so DESTINATION ../lib/) add_definitions(-DUSE_SIFT_GPU) add_definitions(-DSIFT_GPU_MODE=$ENV{SIFT_GPU_MODE}) ENDIF (${USE_SIFT_GPU})

#############################
# GICP ######################
############################# IF (${USE_GICP}) SET(gicp_build_path ${CMAKE_SOURCE_DIR}/external/gicp/ann_1.1.2) FILE(MAKE_DIRECTORY ${gicp_build_path}/lib/) MESSAGE("\n------------------------------------------------------------------\n") MESSAGE("Compiling GICP...") EXECUTE_PROCESS(
COMMAND make ${ROS_PARALLEL_JOBS} linux-g++
WORKING_DIRECTORY ${gicp_build_path}
RESULT_VARIABLE gicp_return
)
MESSAGE("\n------------------------------------------------------------------\n") #on error IF (NOT ${gicp_return} EQUAL 0) MESSAGE(FATAL_ERROR "GICP cannot be compiled. Returned: ${gicp_return}") ENDIF (NOT ${gicp_return} EQUAL 0) FILE(COPY ${gicp_build_path}/lib/libANN.a DESTINATION ${CMAKE_SOURCE_DIR}/lib/) ENDIF (${USE_GICP})

IF (${USE_GICP_BIN}) add_definitions(-DUSE_ICP_BIN) ENDIF (${USE_GICP_BIN})

IF (${USE_GICP_CODE}) add_definitions(-DUSE_ICP_CODE) ENDIF (${USE_GICP_CODE}) IF (${USE_PCL_ICP}) add_definitions(-DUSE_PCL_ICP) ENDIF (${USE_PCL_ICP})

#############################
# OpenCV ####################
############################# find_package(OpenCV)
#include_directories(${OpenCV_INCLUDE_DIRS} external/ /usr/include/suitesparse/) include_directories(${OpenCV_INCLUDE_DIRS} external/ /usr/include/suitesparse/)

#############################
# Eigen ####################
############################# find_package(Eigen) IF(Eigen_FOUND) include_directories(${EIGEN_INCLUDE_DIRS}) add_definitions(${EIGEN_DEFINITIONS}) ELSE(Eigen_FOUND) MESSAGE("Eigen package was not found. This is OK FOR ROS ELECTRIC, bad for fuerte\n") ENDIF(Eigen_FOUND)

##############################################################################
# Some QT Magic (MOC Creation)
##############################################################################

file(GLOB QT_FORMS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} ui/*.ui) file(GLOB QT_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} resources/*.qrc) file(GLOB_RECURSE QT_MOC RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS include/local/*.hpp src/qtros.h src/openni_listener.h src/qt_gui.h src/graph_manager.h src/glviewer.h src/ros_service_ui.h)

QT4_ADD_RESOURCES(QT_RESOURCES_CPP ${QT_RESOURCES}) QT4_WRAP_UI(QT_FORMS_HPP ${QT_FORMS}) QT4_WRAP_CPP(QT_MOC_HPP ${QT_MOC})

##############################################################################
# Sources to Compile
############################################################################## SET(ADDITIONAL_SOURCES src/gicp-fallback.cpp src/main.cpp src/qtros.cpp src/openni_listener.cpp src/qt_gui.cpp src/flow.cpp src/node.cpp src/graph_manager.cpp src/graph_mgr_io.cpp src/glviewer.cpp src/parameter_server.cpp src/ros_service_ui.cpp src/misc.cpp src/landmark.cpp src/loop_closing.cpp src/ColorOctomapServer.cpp src/scoped_timer.cpp src/icp.cpp) SET(ADDITIONAL_SOURCES ${ADDITIONAL_SOURCES} src/transformation_estimation.cpp src/graph_manager2.cpp)

IF (${USE_SIFT_GPU}) SET(ADDITIONAL_SOURCES ${ADDITIONAL_SOURCES} src/sift_gpu_wrapper.cpp) ENDIF (${USE_SIFT_GPU})

file(GLOB_RECURSE QT_SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS ${ADDITIONAL_SOURCES})

##############################################################################
# Binaries
############################################################################## 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 cxsparse -lg2o_cli -lg2o_core
-lg2o_csparse_extension -lg2o_ext_freeglut_minimal -lg2o_incremental
-lg2o_interactive -lg2o_interface -lg2o_opengl_helper -lg2o_parser
-lg2o_simulator -lg2o_solver_cholmod -lg2o_solver_csparse
-lg2o_solver_dense -lg2o_solver_pcg -lg2o_solver_slam2d_linear
-lg2o_solver_structure_only -lg2o_stuff -lg2o_types_data -lg2o_types_icp
-lg2o_types_sba -lg2o_types_sclam2d -lg2o_types_sim3 -lg2o_types_slam2d
-lg2o_types_slam3d)
#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
-loctomap -loctomap_server -loctomath)

IF (${USE_SIFT_GPU}) SET(LIBS_LINK ${LIBS_LINK} siftgpu) ENDIF (${USE_SIFT_GPU}) IF (${USE_GL2PS}) SET(LIBS_LINK ${LIBS_LINK} -lgl2ps) ENDIF (${USE_GL2PS}) IF (${USE_GICP}) SET(LIBS_LINK ${LIBS_LINK} gicp ANN gsl gslcblas) ENDIF (${USE_GICP})
#link libraries target_link_libraries(rgbdslam ${LIBS_LINK})

IF (${USE_GICP}) set(ROS_COMPILE_FLAGS ${ROS_COMPILE_FLAGS} -fpermissive) add_library(gicp external/gicp/bfgs_funcs.cpp external/gicp/gicp.cpp external/gicp/optimize.cpp external/gicp/scan.cpp external/gicp/transform.cpp) ENDIF (${USE_GICP})

## Generate added messages and services with any dependencies listed here
#generate_messages(
#TODO DEPENDENCIES geometry_msgs std_msgs
#)

# catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package
# TODO: fill in what other packages will need to use this package catkin_package(
DEPENDS tf pcl_conversions rospy roscpp rosbag pcl_ros cv_bridge sensor_msgs octomap_server octomap_ros geometry_msgs visualization_msgs libqt4-opengl-dev libqt4-dev libglew-dev libdevil-dev libgsl eigen libg2o octomap
CATKIN_DEPENDS # TODO
INCLUDE_DIRS # TODO include
LIBRARIES # TODO )

package.xml

rgbdslam

0.1.0

This package can be used to register the point clouds from RGBD sensors such as the kinect or stereo cameras.
The created world model can be saved as point cloud or integrated into an octomap.

Felix Endres

Felix Endres

GPL v3

http://ros.org/wiki/rgbdslam

-->

catkin

tf

pcl_conversions

rospy

roscpp

rosbag

pcl_ros

cv_bridge

sensor_msgs

octomap_server

octomap_ros

geometry_msgs

visualization_msgs

tf

pcl_conversions

rospy

roscpp

rosbag

pcl_ros

cv_bridge

sensor_msgs

octomap_server

octomap_ros

geometry_msgs

visualization_msgs

tf
-->
pcl_conversions
-->
rospy
-->
roscpp
-->
rosbag
-->
pcl_ros
-->
cv_bridge
-->
sensor_msgs
-->
octomap_server
-->
octomap_ros
-->
geometry_msgs
-->
visualization_msgs
-->

Now I will paste headers that needs pcl conversions. I have no idea if i made them good
gic-fallback.h

/* This file is part of RGBDSLAM.
*
* RGBDSLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RGBDSLAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RGBDSLAM. If not, see

.
*/

/*
* gicp.cpp
*
* Created on: Jan 23, 2011
* Author: engelhar
*/

#include "gicp-fallback.h"

#include

#include

#include

#include

using namespace std;

void saveCloud(const char* filename, const pointcloud_type& pc, const int max_cnt, const bool color){

ofstream of;
of.open(filename);
assert(of.is_open());

int write_step = 1;
if (max_cnt>0 && (int)pc.points.size()>max_cnt)
write_step = floor(pc.points.size()*1.0/max_cnt);

int cnt = 0;
assert(write_step > 0);

// only write every write_step.th points
for (unsigned int i=0; i
(&p.rgb);
int r = (0xff0000 & color) >> 16;
int g = (0x00ff00 & color) >> 8;
int b = 0x0000ff & color;
of down_sampler;
down_sampler.setLeafSize (0.01, 0.01, 0.01);
pcl::PCLBase
::PointCloudConstPtr const_cloud_ptr = boost::make_shared
(src);
down_sampler.setInputCloud (const_cloud_ptr);
down_sampler.filter(to);
ROS_INFO("gicp.cpp: Downsampling from %i to %i", (int) src.points.size(), (int) to.points.size());
}

bool gicpfallback(const pointcloud_type& from, const pointcloud_type& to, Eigen::Matrix4f& transform){

// std::clock_t starttime_gicp = std::clock();

FILE *fp;
char f1[200];
char f2[200];

char line[130];
char cmd[200];

sprintf(f1, "pc1.txt");
sprintf(f2, "pc2.txt");

// default values for algo work well on this data
sprintf(cmd, "/home/endres/Phd/rospacks/rgbdslam/external/gicp/test_gicp %s %s --d_max 0.1 --debug 0",f1,f2);

int N = 10000;

saveCloud(f1,from,N);
saveCloud(f2,to,N);

// cout lines;

// collect all output
while ( fgets( line, sizeof line, fp))
{
lines.push_back(line);
// ROS_INFO("gicp.cpp: %s", line);
}
int retval = pclose(fp);
// cout ::Identity();
return false;
}

int pos = 0;
// last lines contain the transformation:
for (unsigned int i=lines.size()-5; i
> line;
transform(pos,j) = atof(line);
}
// cout > line; // Converged in
ss >> line;
ss >> line;

int iter_cnt;
iter_cnt = atoi(line);

return iter_cnt .
*/

#ifndef RGBD_SLAM_NODE_H_
#define RGBD_SLAM_NODE_H_

#include

#include "ros/ros.h"
#include

#include

#include

#include

//#include

#include

#include

#include

#include

#include "parameter_server.h"
//for ground truth
#include

#include

#ifdef USE_ICP_BIN
#include "gicp-fallback.h"
#endif

#ifdef USE_ICP_CODE
#include "gicp/gicp.h"
#include "gicp/transform.h"
#endif

#include "matching_result.h"
#include

typedef std::vector
> std_vector_of_eigen_vector4f;
//!Holds the data for one graph node and provides functionality to compute relative transformations to other Nodes.
class Node {
public:
///Visual must be CV_8UC1, depth CV_32FC1,
///detection_mask must be CV_8UC1 with non-zero
///at potential keypoint locations
Node(const cv::Mat& visual,
const cv::Mat& depth,
const cv::Mat& detection_mask,
const sensor_msgs::CameraInfoConstPtr& cam_info,
std_msgs::Header depth_header,
cv::Ptr
detector,
cv::Ptr
extractor);
///Visual must be CV_8UC1
///detection_mask must be CV_8UC1 with non-zero
///at potential keypoint locations
Node(const cv::Mat visual,
cv::Ptr
detector,
cv::Ptr
extractor,
pointcloud_type::Ptr point_cloud,
const cv::Mat detection_mask = cv::Mat());
//default constructor. TODO: still needed?
Node(){}
///Delete the flannIndex if built
~Node();

///Compare the features of two nodes and compute the transformation
MatchingResult matchNodePair(const Node* older_node);
//MatchingResult matchNodePair2(const Node* older_node);

///Transform, e.g., from Joint/Wheel odometry
void setOdomTransform(tf::StampedTransform odom);
///Transform, e.g., from MoCap
void setGroundTruthTransform(tf::StampedTransform gt);
///Transform, e.g., from kinematics
void setBase2PointsTransform(tf::StampedTransform& b2p);
///Transform, e.g., from Joint/Wheel odometry
tf::StampedTransform getOdomTransform() const;
///Transform, e.g., from MoCap
tf::StampedTransform getGroundTruthTransform() const;
///Transform, e.g., from kinematics
tf::StampedTransform getBase2PointsTransform() const;

///Compute the relative transformation between the nodes
bool getRelativeTransformationTo(const Node* target_node,
std::vector
* initial_matches,
Eigen::Matrix4f& resulting_transformation,
float& rmse,
std::vector
& matches) const;

#ifdef USE_ICP_BIN
// initial_transformation: optional transformation applied to this->pc before
// using icp
bool getRelativeTransformationTo_ICP_bin(const Node* target_node,Eigen::Matrix4f& transformation,
const Eigen::Matrix4f* initial_transformation = NULL);
#endif

//!Fills "matches" and returns ratio of "good" features
//!in the sense of distinction via the "nn_distance_ratio" setting (see parameter server)
unsigned int featureMatching(const Node* other, std::vector
* matches) const;

#ifdef USE_ICP_CODE
bool getRelativeTransformationTo_ICP_code(const Node* target_node,
Eigen::Matrix4f& transformation,
const Eigen::Matrix4f& initial_transformation);

static const double gicp_epsilon = 1e-3;
static const double gicp_d_max_ = 5.0; // 10cm
static const int gicp_max_iterations = 10;
static const int gicp_min_point_cnt = 100;

bool gicp_initialized;
void Eigen2GICP(const Eigen::Matrix4f& m, dgc_transform_t g_m);
void GICP2Eigen(const dgc_transform_t g_m, Eigen::Matrix4f& m);
void gicpSetIdentity(dgc_transform_t m);
dgc::gicp::GICPPointSet* getGICPStructure(unsigned int max_count = 0) const;
void clearGICPStructure() const;
protected:
mutable dgc::gicp::GICPPointSet* gicp_point_set_;

public:
#endif

void clearFeatureInformation();
void addPointCloud(pointcloud_type::Ptr pc_col);

//!erase the points from the cloud to save memory
void clearPointCloud();
//!reduce the points from the cloud using a voxelgrid_filter to save memory
void reducePointCloud(double voxelfilter_size);
//PointCloud pc;
///pointcloud_type centrally defines what the pc is templated on
int id_; //
siftgpu_descriptors;

///Where in the image are the descriptors
std::vector
feature_locations_2d_;
///Contains the minimum and maximum depth in the feature's range (not used yet)
std::vector
> feature_depth_stats_;
///Contains how often a feature has been an (inlier) match
std::vector
feature_matching_stats_;

#ifdef DO_FEATURE_OPTIMIZATION

std::map
kpt_to_landmark;
// std::set
visible_landmarks;
#endif

const cv::flann::Index* getFlannIndex() const;
void knnSearch(cv::Mat& query,
cv::Mat& indices,
cv::Mat& dists,
int knn,
const cv::flann::SearchParams& params) const
{
this->flannIndex->knnSearch(query, indices, dists, knn, params);
}

long getMemoryFootprint(bool print);
protected:
static QMutex gicp_mutex;
static QMutex siftgpu_mutex;
mutable cv::flann::Index* flannIndex;
tf::StampedTransform base2points_; //!
keypoints);

#ifdef USE_SIFT_GPU
//! return the 3D projection of valid keypoints using information from the point cloud and remove invalid keypoints (NaN depth)
void projectTo3DSiftGPU(std::vector
& feature_locations_2d,
std::vector
>& feature_locations_3d,
const pointcloud_type::Ptr point_cloud,
std::vector
& descriptors_in, cv::Mat& descriptors_out);
//! return the 3D projection of valid keypoints using information from the depth image and remove invalid keypoints (NaN depth)
void projectTo3DSiftGPU(std::vector
& feature_locations_2d,
std::vector
>& feature_locations_3d,
const cv::Mat& depth,
const sensor_msgs::CameraInfoConstPtr& cam_info,
std::vector
& descriptors_in, cv::Mat& descriptors_out);
#endif
//! return the 3D projection of valid keypoints using information from the point cloud and remove invalid keypoints (NaN depth)
void projectTo3D(std::vector
& feature_locations_2d,
std::vector
>& feature_locations_3d,
const pointcloud_type::ConstPtr point_cloud);
//! return the 3D projection of valid keypoints using information from the depth image and remove invalid keypoints (NaN depth)
void projectTo3D(std::vector
& feature_locations_2d,
std::vector
>& feature_locations_3d,
const cv::Mat& depth,
const sensor_msgs::CameraInfoConstPtr& cam_info);

///Retrieves and stores the transformation from base to point cloud at capturing time
void retrieveBase2CamTransformation();
// helper for ransac
void computeInliersAndError(const std::vector
& initial_matches,
const Eigen::Matrix4f& transformation,
const std::vector
>& origins,
//const std::vector
> origins_depth_stats,
const std::vector
>& targets,
//const std::vector
> targets_depth_stats,
std::vector
& new_inliers, //pure output var
double& mean_error, //pure output var //std::vector
& errors,
double squaredMaxInlierDistInM = 0.0009) const; //output var;

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void pairwiseObservationLikelihood(const Node* newer_node, const Node* older_node, MatchingResult& mr);
///Compute the RootSIFT from SIFT according to Arandjelovic and Zisserman
void squareroot_descriptor_space(cv::Mat& feature_descriptors);
// Compute the transformation from matches using pcl::TransformationFromCorrespondences
Eigen::Matrix4f getTransformFromMatches(const Node* newer_node,
const Node* older_node,
const std::vector
& matches,
bool& valid,
float max_dist_m = -1);

// Compute the transformation from matches using Eigen::umeyama
Eigen::Matrix4f getTransformFromMatchesUmeyama(const Node* newer_node,
const Node* older_node,
std::vector
matches);

#endif

parameter_server.h

/* This file is part of RGBDSLAM.
*
* RGBDSLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RGBDSLAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RGBDSLAM. If not, see

.
*/
#ifndef PARAMETER_SERVER_H_
#define PARAMETER_SERVER_H_
#include

#include

#include

//this is a global definition of the points to be used
//changes to omit color would need adaptations in
//the visualization too
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#ifndef RGB_IS_4TH_DIM
typedef pcl::PointXYZRGB point_type;
#else
typedef pcl::PointXYZ point_type;
#endif
typedef pcl::PointCloud
pointcloud_type;
//#define CONCURRENT_EDGE_COMPUTATION
//Compile out DEBUG Statements. Hardly benefitial though
#define ROSCONSOLE_SEVERITY_INFO 1

/*!
* \brief Getting values from parameter server.
* This class is used for getting the parameters from
* the parameter server
*/
class ParameterServer {
public:
/*!
* Returns the singleton instance
*/
static ParameterServer* instance();

/*!
* The method sets a value in the local cache and on the Parameter Server.
* You can use bool, int, double and std::string for T
*
* \param param the name of the parameter
* \value the new parameter value
*/
template

void set(const std::string param, T value) {
if(config.count(param)==0){
ROS_ERROR("ParameterServer: Ignoring invalid parameter: \"%s\"", param.c_str());
return;
}
try{
boost::any_cast
(value); //fails if wrong param type
} catch (boost::bad_any_cast e) {
ROS_ERROR("ParameterServer: Ignoring invalid parameter type: %s", e.what());
return;
}
config[param] = value;
setOnParameterServer(pre+param, value);
}

/*!
* The method returns a value from the local cache.
* You can use bool, int, double and std::string for T
*
* \param param the name of the parameter
* \return the parameter value
*/
template

T get(const std::string param) {
if(config.count(param)==0){
ROS_FATAL("ParameterServer object queried for invalid parameter \"%s\"", param.c_str());
assert(config.count(param)==0);
}
boost::any value = config[param];
try{
return boost::any_cast
(value);
} catch( boost::bad_any_cast bac){
ROS_ERROR_STREAM("Bad cast: Requested data type for parameter '" & getConfigData(){
return config;
}

/*!
* Receives all values from the parameter server and store them
* in the map 'config'.
* Will be called in the constructor
*/
void getValues();
private:
void addOption(std::string name, boost::any value, std::string description);
std::map
config;
std::map
descriptions;

static ParameterServer* _instance;
std::string pre;
ros::NodeHandle handle;

/*!
* Default constructor
* private, because of singleton
*/
ParameterServer();

/*!
* Loads the default configuration
*/
void defaultConfig();

/*!
* Checks, whether the parameters are ok
*/
void checkValues();

/*!
* Returns a value from the parameter server
* Will only be used by getValue()
*
* \param param name of the parameter
* \param def default value (get through defaultConfig())
*
* \return the parameter value
*/
template

T getFromParameterServer(const std::string param, T def) {
T result;
handle.param(param, result, def);
return result;
}

template

void setOnParameterServer(const std::string param, T new_val) {
handle.setParam(param, new_val);
}
};

#endif /* PARAMETER_SERVER_H_ */

Recently I have found that I dont have libgsl. To install it the easies way is:
sudo apt-get install gsl-bin libgsl0-dev

libqt4, libglev, libdevil and libg2o are not catkin dependencies right?
hmm its seems like g2o is catkin dependency but doesn't provieds it pkg-config file yet ;/

Still got other problems with other things. Please help

I was playing a bit with CmakeLists and now i get this error:

CMake Error at rgbdslam_freiburg/rgbdslam/CMakeLists.txt:83 (target_link_libraries):
Cannot specify link libraries for target "rgbdslam" which is not built by
this project.

-- Configuring incomplete, errors occurred!
make: *** [cmake_check_build_system] Błąd 1
Invoking "make cmake_check_build_system" failed

My CmakeLists looks like that:

# Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html
# Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html
cmake_minimum_required(VERSION 2.8.3)
project(rgbdslam)
# Load catkin and all dependencies required for this package
# TODO: remove all from COMPONENTS that are not catkin packages.
find_package(catkin REQUIRED COMPONENTS tf pcl_conversions rospy roscpp rosbag pcl_ros cv_bridge sensor_msgs octomap_server octomap_ros geometry_msgs visualization_msgs eigen octomap)

# include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})#########################################################
### CONFIG ##############################################
#########################################################
# 1, if SiftGPU should be used. For additional information
# see README
set(USE_SIFT_GPU 1)
set(ENV{SIFT_GPU_MODE} 2) #CUDA = 1, GLSL = 2
set(ENV{SIFT_GPU_CUDA_PATH} /usr/local/cuda)

set(USE_GICP_BIN 0)
set(USE_GICP_CODE 0)
set(USE_PCL_ICP 1)
set(USE_GL2PS 0) #For printing the 3D Window to PostScript - requires installation of additional library: libgl2ps-dev
IF ("${ROS_PARALLEL_JOBS}" EQUAL "")
set(ROS_PARALLEL_JOBS 1)
endif ("${ROS_PARALLEL_JOBS}" EQUAL "")
#########################################################
#########################################################
#########################################################

set(ROS_COMPILE_FLAGS ${ROS_COMPILE_FLAGS} -fopenmp)

# CATKIN_MIGRATION: removed during catkin migration
# cmake_minimum_required(VERSION 2.4.6)

# CATKIN_MIGRATION: removed during catkin migration
# 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
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
set(ROS_BUILD_TYPE RelWithDebInfo)
#add_definitions(-DROSCONSOLE_SEVERITY_INFO)
IF (${USE_GL2PS})
add_definitions(-DGL2PS)
ENDIF (${USE_GL2PS})
set(CMAKE_CXX_FLAGS "-O3")
#is used for compiling the libraries
set(USE_GICP 0)
IF (${USE_GICP_BIN} OR ${USE_GICP_CODE})
set(USE_GICP 1)
ENDIF (${USE_GICP_BIN} OR ${USE_GICP_CODE})

# CATKIN_MIGRATION: removed during catkin migration
# rosbuild_init()

#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

#uncomment if you have defined messages
#add_message_files(
# FILES
# TODO: List your msg files here
#)
#uncomment if you have defined services
add_service_files(
FILES
# TODO: List your msg files here
)
#common commands for building c++ executables and libraries
#add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#
# CATKIN_MIGRATION: removed during catkin migration
# rosbuild_add_boost_directories()
#find_package(Boost REQUIRED COMPONENTS thread)
include_directories(${Boost_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES})
#add_executable(example examples/example.cpp)

##############################################################################
# Required 3rd party
##############################################################################

#############################
# Octomap ###################
#############################
find_package(octomap REQUIRED)
include_directories(${OCTOMAP_INCLUDE_DIRS})
link_directories(${OCTOMAP_LIBRARY_DIRS})
link_libraries(${OCTOMAP_LIBRARIES})

#############################
# Qt ########################
#############################
# http://qtnode.net/wiki/Qt4_with_cmake
find_package(Qt4 REQUIRED)
SET(QT_USE_QTXML TRUE)
SET(QT_USE_QTOPENGL TRUE)
ADD_DEFINITIONS(-DQT_NO_KEYWORDS)
include(${QT_USE_FILE})

# This is necessary as all ui files etc will get dumped in the bottom of then binary directory.
include_directories(${CMAKE_CURRENT_BINARY_DIR} ${QT_QTOPENGL_INCLUDE_DIR} external/gicp/ann_1.1.2/include/ANN)

#get_directory_property(clean ADDITIONAL_MAKE_CLEAN_FILES)
#SET(clean external/siftgpu/linux/bin/libsiftgpu.so)
#set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${clean}")

LINK_DIRECTORIES(${LIBRARY_OUTPUT_PATH})
SET(${CMAKE_LIBRARY_PATH} ${LIBRARY_OUTPUT_PATH})
#SET(CMAKE_INCLUDE_DIRECTORIES_PROJECT_BEFORE TRUE)

ADD_DEFINITIONS(-DDO_FEATURE_OPTIMIZATION)

#Only possible when not subscribing to point cloud topic. Possibly dangerous anyway, but halves storage space per point cloud
#ADD_DEFINITIONS(-DRGB_IS_4TH_DIM)

#############################
# SiftGPU ###################
#############################
IF (${USE_SIFT_GPU})
SET(sift_gpu_build_path ${CMAKE_SOURCE_DIR}/external/siftgpu/linux)

include_directories(${CMAKE_CURRENT_BINARY_DIR} external/siftgpu/src)

MESSAGE("\n------------------------------------------------------------------\n")
MESSAGE("Compiling SiftGPU...")
EXECUTE_PROCESS(
COMMAND make ${ROS_PARALLEL_JOBS} siftgpu
WORKING_DIRECTORY ${sift_gpu_build_path}
RESULT_VARIABLE sift_gpu_return
)
MESSAGE("\n------------------------------------------------------------------\n")

#on error
IF (NOT ${sift_gpu_return} EQUAL 0)
MESSAGE(FATAL_ERROR "SiftGPU cannot be compiled. Returned: ${sift_gpu_return}")
ENDIF (NOT ${sift_gpu_return} EQUAL 0)

FILE(COPY external/siftgpu/linux/bin/libsiftgpu.so DESTINATION ../lib/)

add_definitions(-DUSE_SIFT_GPU)
add_definitions(-DSIFT_GPU_MODE=$ENV{SIFT_GPU_MODE})
ENDIF (${USE_SIFT_GPU})

#############################
# GICP ######################
#############################
IF (${USE_GICP})
SET(gicp_build_path ${CMAKE_SOURCE_DIR}/external/gicp/ann_1.1.2)
FILE(MAKE_DIRECTORY ${gicp_build_path}/lib/)
MESSAGE("\n------------------------------------------------------------------\n")
MESSAGE("Compiling GICP...")
EXECUTE_PROCESS(
COMMAND make ${ROS_PARALLEL_JOBS} linux-g++
WORKING_DIRECTORY ${gicp_build_path}
RESULT_VARIABLE gicp_return
)
MESSAGE("\n------------------------------------------------------------------\n")

#on error
IF (NOT ${gicp_return} EQUAL 0)
MESSAGE(FATAL_ERROR "GICP cannot be compiled. Returned: ${gicp_return}")
ENDIF (NOT ${gicp_return} EQUAL 0)

FILE(COPY ${gicp_build_path}/lib/libANN.a DESTINATION ${CMAKE_SOURCE_DIR}/lib/)
ENDIF (${USE_GICP})

IF (${USE_GICP_BIN})
add_definitions(-DUSE_ICP_BIN)
ENDIF (${USE_GICP_BIN})

IF (${USE_GICP_CODE})
add_definitions(-DUSE_ICP_CODE)
ENDIF (${USE_GICP_CODE})
IF (${USE_PCL_ICP})
add_definitions(-DUSE_PCL_ICP)
ENDIF (${USE_PCL_ICP})

#############################
# OpenCV ####################
#############################
find_package(OpenCV)
#include_directories(${OpenCV_INCLUDE_DIRS} external/ /usr/include/suitesparse/)
include_directories(${OpenCV_INCLUDE_DIRS} external/ /usr/include/suitesparse/)

#############################
# Eigen ####################
#############################
find_package(Eigen)
IF(Eigen_FOUND)
include_directories(${EIGEN_INCLUDE_DIRS})
add_definitions(${EIGEN_DEFINITIONS})
ELSE(Eigen_FOUND)
MESSAGE("Eigen package was not found. This is OK FOR ROS ELECTRIC, bad for fuerte\n")
ENDIF(Eigen_FOUND)

##############################################################################
# Some QT Magic (MOC Creation)
##############################################################################

file(GLOB QT_FORMS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} ui/*.ui)
file(GLOB QT_RESOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} resources/*.qrc)
file(GLOB_RECURSE QT_MOC RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS include/local/*.hpp src/qtros.h src/openni_listener.h src/qt_gui.h src/graph_manager.h src/glviewer.h src/ros_service_ui.h)

QT4_ADD_RESOURCES(QT_RESOURCES_CPP ${QT_RESOURCES})
QT4_WRAP_UI(QT_FORMS_HPP ${QT_FORMS})
QT4_WRAP_CPP(QT_MOC_HPP ${QT_MOC})

##############################################################################
# Sources to Compile
##############################################################################
SET(ADDITIONAL_SOURCES src/gicp-fallback.cpp src/main.cpp src/qtros.cpp src/openni_listener.cpp src/qt_gui.cpp src/flow.cpp src/node.cpp src/graph_manager.cpp src/graph_mgr_io.cpp src/glviewer.cpp src/parameter_server.cpp src/ros_service_ui.cpp src/misc.cpp src/landmark.cpp src/loop_closing.cpp src/ColorOctomapServer.cpp src/scoped_timer.cpp src/icp.cpp)
SET(ADDITIONAL_SOURCES ${ADDITIONAL_SOURCES} src/transformation_estimation.cpp src/graph_manager2.cpp)

IF (${USE_SIFT_GPU})
SET(ADDITIONAL_SOURCES ${ADDITIONAL_SOURCES} src/sift_gpu_wrapper.cpp)
ENDIF (${USE_SIFT_GPU})

file(GLOB_RECURSE QT_SOURCES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS ${ADDITIONAL_SOURCES})

##############################################################################
# Binaries
##############################################################################
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 cxsparse -lg2o_cli -lg2o_core
-lg2o_csparse_extension -lg2o_ext_freeglut_minimal -lg2o_incremental
-lg2o_interactive -lg2o_interface -lg2o_opengl_helper -lg2o_parser
-lg2o_simulator -lg2o_solver_cholmod -lg2o_solver_csparse
-lg2o_solver_dense -lg2o_solver_pcg -lg2o_solver_slam2d_linear
-lg2o_solver_structure_only -lg2o_stuff -lg2o_types_data -lg2o_types_icp
-lg2o_types_sba -lg2o_types_sclam2d -lg2o_types_sim3 -lg2o_types_slam2d
-lg2o_types_slam3d)
#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 -loctomap -loctomap_server -loctomath)

IF (${USE_SIFT_GPU})
SET(LIBS_LINK ${LIBS_LINK} siftgpu)
ENDIF (${USE_SIFT_GPU})
IF (${USE_GL2PS})
SET(LIBS_LINK ${LIBS_LINK} -lgl2ps)
ENDIF (${USE_GL2PS})
IF (${USE_GICP})
SET(LIBS_LINK ${LIBS_LINK} gicp ANN gsl gslcblas)
ENDIF (${USE_GICP})
#link libraries
target_link_libraries(rgbdslam ${LIBS_LINK})

IF (${USE_GICP})
set(ROS_COMPILE_FLAGS ${ROS_COMPILE_FLAGS} -fpermissive)
add_library(gicp external/gicp/bfgs_funcs.cpp external/gicp/gicp.cpp external/gicp/optimize.cpp external/gicp/scan.cpp external/gicp/transform.cpp)
ENDIF (${USE_GICP})

## Generate added messages and services with any dependencies listed here
generate_messages(
#TODO DEPENDENCIES geometry_msgs std_msgs
)

# catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package
# TODO: fill in what other packages will need to use this package
catkin_package(
DEPENDS tf pcl_conversions rospy roscpp rosbag pcl_ros cv_bridge sensor_msgs octomap_server octomap_ros geometry_msgs visualization_msgs libqt4-opengl-dev libqt4-dev libglew-dev libdevil-dev libgsl eigen libg2o octomap
CATKIN_DEPENDS # TODO
INCLUDE_DIRS # TODO include
LIBRARIES # TODO
)

Show more