m4nh / skimap_ros Goto Github PK

@m4nh Thanks for the great work! As I gone through the code, the SaveToFile() seen like only store the points. I curious what is output file format and is the output file only suitable for rviz other than other rendering engine? Thanks.

Hi @m4nh , Thanks for your excellent job, But the tiago_lar.bag does not exist. Can you provide a new data set connection? Thanks a lot.

Hi @m4nh ,
The node dies after a sometime.
Debugging using GDB I got the following error:

> Thread 1 "skimap_path_pla" received signal SIGSEGV, Segmentation fault.
> 0x00000000004755db in skimap::SkipListMapV2<skimap::VoxelDataRGBW<short, unsigned short>, short, float, 8, 8, 8>::integrateVoxel(short, short, short, skimap::VoxelDataRGBW<short, unsigned short>*) ()

I guess is something related with threading and OpenMP, but I am not sure.
Thanks in advance,
Bruno

Hi! I'm trying to use the slamdunk_tracker.

However, I got the error :

[slamdunk_tracker-1] process has died [pid 7893, exit code -11, cmd /home/user/catkin_ws/devel/lib/skimap_ros/slamdunk_tracker __name:=slamdunk_tracker __log:=/home/user/.ros/log/947ab95c-37a6-11e7-8cec-2c4d545321e9/slamdunk_tracker-1.log].
log file: /home/user/.ros/log/947ab95c-37a6-11e7-8cec-2c4d545321e9/slamdunk_tracker-1*.log

I found out that segmentation fault happens at slamdunk_tracker.cpp, line 84:

slamdunk::SlamDunkScene slam_dunk_scene(slam_dunk);

How can I solve this problem?

Hi @m4nh ,
Is there any example how to build 2.5 D map?
I was trying to use 2D grid z coordinate as elevation, but this is constant. Can you help please?
Thanks,
Bruno

Hi,
Is there an integration with velodyne lidar? Or just with RGB-D cameras?
Thanks in advance,
Bruno

Hi,
I'm trying to compile skimap + slamdunk on Ubuntu 14.04 and ROS indigo and catkin tools.
I installed g2o into the folder /skimap_ros/thirdparty/, compiled the lib from source and added to CMakeList with SET( G2O_ROOT ${PROJECT_SOURCE_DIR}/thirdparty/g2o).
I also installed opencv2.4 non free packages.
Which version of Eigen are you using?

I can compile SKIMAP but I have this issue when I try to include SLAM_DUNK:
In file included from /usr/local/include/eigen3/Eigen/Core:343:0, from /usr/local/include/eigen3/Eigen/Geometry:11, from /home/andrea/development/ros/catkin_tools_ws/src/skimap_ros/include/slamdunk/graph_utils.h:13, from /home/andrea/development/ros/catkin_tools_ws/src/skimap_ros/include/slamdunk/graph_backend.h:13, from /home/andrea/development/ros/catkin_tools_ws/src/skimap_ros/src/slamdunk/graph_backend.cpp:9: /usr/local/include/eigen3/Eigen/src/Core/CoreEvaluators.h: In instantiation of ‘Eigen::internal::evaluator<Eigen::PartialReduxExpr<ArgType, MemberOp, Direction> >::evaluator(Eigen::internal::evaluator<Eigen::PartialReduxExpr<ArgType, MemberOp, Direction> >::XprType) [with ArgType = const Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, const Eigen::Matrix<double, 0, 0, 0, 0, 0> >; MemberOp = Eigen::internal::member_sum<double>; int Direction = 0; Eigen::internal::evaluator<Eigen::PartialReduxExpr<ArgType, MemberOp, Direction> >::XprType = Eigen::PartialReduxExpr<const Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, const Eigen::Matrix<double, 0, 0, 0, 0, 0> >, Eigen::internal::member_sum<double>, 0>]’: /usr/local/include/eigen3/Eigen/src/Core/Redux.h:338:95: required from ‘Eigen::internal::redux_evaluator<_XprType>::redux_evaluator(const XprType&) [with _XprType = Eigen::PartialReduxExpr<const Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, const Eigen::Matrix<double, 0, 0, 0, 0, 0> >, Eigen::internal::member_sum<double>, 0>; Eigen::internal::redux_evaluator<_XprType>::XprType = Eigen::PartialReduxExpr<const Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, const Eigen::Matrix<double, 0, 0, 0, 0, 0> >, Eigen::internal::member_sum<double>, 0>]’ /usr/local/include/eigen3/Eigen/src/Core/Redux.h:416:35: required from ‘typename Eigen::internal::traits<T>::Scalar Eigen::DenseBase<Derived>::redux(const Func&) const [with BinaryOp = Eigen::internal::scalar_max_op<double, double>; Derived = Eigen::PartialReduxExpr<const Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, const Eigen::Matrix<double, 0, 0, 0, 0, 0> >, Eigen::internal::member_sum<double>, 0>; typename Eigen::internal::traits<T>::Scalar = double]’ /usr/local/include/eigen3/Eigen/src/Core/Redux.h:438:73: required from ‘typename Eigen::internal::traits<T>::Scalar Eigen::DenseBase<Derived>::maxCoeff() const [with Derived = Eigen::PartialReduxExpr<const Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, const Eigen::Matrix<double, 0, 0, 0, 0, 0> >, Eigen::internal::member_sum<double>, 0>; typename Eigen::internal::traits<T>::Scalar = double]’ /usr/local/include/eigen3/Eigen/src/LU/PartialPivLU.h:522:13: required from ‘void Eigen::PartialPivLU<MatrixType>::compute() [with _MatrixType = Eigen::Matrix<double, 0, 0, 0, 0, 0>]’ /usr/local/include/eigen3/Eigen/src/LU/PartialPivLU.h:131:15: required from ‘Eigen::PartialPivLU<MatrixType>& Eigen::PartialPivLU<MatrixType>::compute(const Eigen::EigenBase<OtherDerived>&) [with InputType = Eigen::Matrix<double, 0, 0, 0, 0, 0>; _MatrixType = Eigen::Matrix<double, 0, 0, 0, 0, 0>]’ /usr/local/include/eigen3/Eigen/src/LU/PartialPivLU.h:323:27: [ skipping 5 instantiation contexts, use -ftemplate-backtrace-limit=0 to disable ] /usr/local/include/eigen3/Eigen/src/Core/AssignEvaluator.h:787:42: required from ‘void Eigen::internal::call_assignment(Dst&, const Src&, const Func&, typename Eigen::internal::enable_if<(! Eigen::internal::evaluator_assume_aliasing<Src>::value), void*>::type) [with Dst = Eigen::Matrix<double, 0, 0, 0, 0, 0>; Src = Eigen::Inverse<Eigen::Matrix<double, 0, 0, 0, 0, 0> >; Func = Eigen::internal::assign_op<double, double>; typename Eigen::internal::enable_if<(! Eigen::internal::evaluator_assume_aliasing<Src>::value), void*>::type = void*]’ /usr/local/include/eigen3/Eigen/src/Core/AssignEvaluator.h:765:93: required from ‘void Eigen::internal::call_assignment(Dst&, const Src&) [with Dst = Eigen::Matrix<double, 0, 0, 0, 0, 0>; Src = Eigen::Inverse<Eigen::Matrix<double, 0, 0, 0, 0, 0> >]’ /usr/local/include/eigen3/Eigen/src/Core/PlainObjectBase.h:710:65: required from ‘Derived& Eigen::PlainObjectBase<Derived>::_set(const Eigen::DenseBase<OtherDerived>&) [with OtherDerived = Eigen::Inverse<Eigen::Matrix<double, 0, 0, 0, 0, 0> >; Derived = Eigen::Matrix<double, 0, 0, 0, 0, 0>]’ /usr/local/include/eigen3/Eigen/src/Core/Matrix.h:225:30: required from ‘Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::operator=(const Eigen::DenseBase<OtherDerived>&) [with OtherDerived = Eigen::Inverse<Eigen::Matrix<double, 0, 0, 0, 0, 0> >; _Scalar = double; int _Rows = 0; int _Cols = 0; int _Options = 0; int _MaxRows = 0; int _MaxCols = 0]’ /home/andrea/development/ros/catkin_tools_ws/src/skimap_ros/thirdparty/g2o/g2o/core/block_solver.hpp:370:10: required from ‘bool g2o::BlockSolver<Traits>::solve() [with Traits = g2o::BlockSolverTraits<6, 0>]’ /home/andrea/development/ros/catkin_tools_ws/src/skimap_ros/src/slamdunk/graph_backend.cpp:417:1: required from here /usr/local/include/eigen3/Eigen/src/Core/util/StaticAssert.h:32:40: error: static assertion failed: EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT__INVALID_COST_VALUE #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG); ^ /usr/local/include/eigen3/Eigen/src/Core/util/StaticAssert.h:214:7: note: in expansion of macro ‘EIGEN_STATIC_ASSERT’ EIGEN_STATIC_ASSERT((C)>=0 && (C)<=HugeCost*HugeCost, EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT__INVALID_COST_VALUE); ^ /usr/local/include/eigen3/Eigen/src/Core/CoreEvaluators.h:1280:5: note: in expansion of macro ‘EIGEN_INTERNAL_CHECK_COST_VALUE’ EIGEN_INTERNAL_CHECK_COST_VALUE(TraversalSize==Dynamic ? HugeCost : int(CostOpType::value)); ^ /usr/local/include/eigen3/Eigen/src/Core/util/StaticAssert.h:32:40: error: static assertion failed: EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT__INVALID_COST_VALUE #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG); ^ /usr/local/include/eigen3/Eigen/src/Core/util/StaticAssert.h:214:7: note: in expansion of macro ‘EIGEN_STATIC_ASSERT’ EIGEN_STATIC_ASSERT((C)>=0 && (C)<=HugeCost*HugeCost, EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT__INVALID_COST_VALUE); ^ /usr/local/include/eigen3/Eigen/src/Core/CoreEvaluators.h:1281:5: note: in expansion of macro ‘EIGEN_INTERNAL_CHECK_COST_VALUE’ EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost); ^ make[2]: *** [CMakeFiles/slamdunk.dir/src/slamdunk/graph_backend.cpp.o] Errore 1 make[1]: *** [CMakeFiles/slamdunk.dir/all] Errore 2 make: *** [all] Errore 2
Tnks
Andrea

Can skimap be integrated with PTAM's published pose. By creating a node that takes the ptam pose and resend it as tf. I am currently having issues getting PTAM to compile/run on my system , I will try this when I get it resolved but I wanted to know if there any issues that would stop this from working.

Hi, thanks for offering the great project! I'm newbie at ROS, when I trying to plug skimap into Orb slam2, I don't know where to add the code to trans the point and senser data to skimap, any advice will be helpful! Thanks!

Hi,

I followed the discussion in ORBSLAM 2 and saw the following clip:
https://www.youtube.com/watch?v=W3nm2LXmgqE

I can run ORBSLAM and Skimap live, but dont have any idea how to integrate them, partially because there are no topics from ORBSLAM 2. I appreciate if you could give me some idea and explanation about it.

Hi,
I also found there is an error when run this command : roslaunch skimap_ros slamdunk_tracker.launch
ERROR: cannot launch node of type [skimap_ros/slamdunk_tracker]: can't locate node [slamdunk_tracker] in package [skimap_ros]

I found that in CmakeList.txt there is no add_executable about slamdunk_tracker.cpp

add_executable(skimap_live src/nodes/skimap_live.cpp)
target_link_libraries(skimap_live ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})

add_executable(skimap_map_service src/nodes/skimap_map_service.cpp)
target_link_libraries(skimap_map_service ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})
add_dependencies(skimap_map_service skimap_ros_generate_messages_cpp)

What do you think?

Hi, m4nh

I have run the demo, but cannot see the octomap. Do you have any idea?
Thanks very much.

Hi,
I read the service code to adapt for my own purposes. Regarding the following piece of code,

tf::Transform base_to_camera = tf::Transform(
      tf::Quaternion(
          req.sensor_pose.orientation.x, req.sensor_pose.orientation.y,
          req.sensor_pose.orientation.z, req.sensor_pose.orientation.w),
      tf::Vector3(req.sensor_pose.position.x, req.sensor_pose.position.y,
                  req.sensor_pose.position.z));

  tf::Quaternion q = base_to_camera.getRotation();
tf::Vector3 v = base_to_camera.getOrigin();

Does this transformation helps improving the mapping ? If so, could you detail how does it work in general terms?
Thanks a lot.
Regards,
Bruno

First of all, thanks for the awesome tool.
I'm trying to build a map of a synthetic scene using SkiMap. To do so, I convert my own data into bag file and replay it. Resulting point clouds look decent to me:

However, whole map gets filled with noisy obstacles quite fast:

The black thing in front of the agent should be a few voxels thick, but it's drifting a lot.

Definitely, there is something fishy about agent's poses, which I have to convert from my own environment to /tf. I use simplified version of a transformation tree that includes just map, odom, base_link and optical_frame (see figures above). I fix agent's pose at the very first frame and during map reconstruction I store differences between initial state and a current one:

        cur_position = position - init_pose

        cur_orientation = quaternion_multiply(init_orient, quaternion_conjugate(orientation))

        cur_pose = {cur_position, cur_orientation}

        baselink_tf = pose_to_tf(cur_pose, 'odom', 'base_link', timestamp)

Could you provide any insights about what could I try?

Hey i successfully made skimap to work with orb slam 2 ...but i have some confusions.. i saw some parameters in launch file called ground height and agent height... i cant understand what are those...what i think is a quadrotor doesnt fly at constant height so the agent height cant be constant, anyways while testing i set them to zero. If i am thinking wrong please correct me....can it be used with quadrotors too?
I got a map something like this

Hi,
running rosbag play tiago_lar.bag --clock --pause I noticed that there is a difference of 111.7 seconds between the bag time and messages timestamps.

This can be verified with the following python code:

import rosbag
msg_counter = 1
for topic, msg, t in rosbag.Bag('tiago_lar.bag').read_messages():
    #if topic != "/tf":
    if topic == "/xtion/rgb/image_raw":
        print("Topic: {}".format(topic))
        print("t.to_sec(): {}".format(t.to_sec()))
        print("msg.header.stamp.to_sec(): {}".format(msg.header.stamp.to_sec()))
        print("Delta time: {}".format(t.to_sec() - (msg.header.stamp.to_sec())))
        print("")
        msg_counter = msg_counter + 1
    if msg_counter == 5:
        break

The code produce the following output:

Topic: /xtion/rgb/image_raw
t.to_sec(): 1472902408.28
msg.header.stamp.to_sec(): 1472902296.56
Delta time: 111.716982841

Topic: /xtion/rgb/image_raw
t.to_sec(): 1472902408.37
msg.header.stamp.to_sec(): 1472902296.66
Delta time: 111.707210302

Topic: /xtion/rgb/image_raw
t.to_sec(): 1472902408.73
msg.header.stamp.to_sec(): 1472902297.0
Delta time: 111.730161905

Topic: /xtion/rgb/image_raw
t.to_sec(): 1472902408.87
msg.header.stamp.to_sec(): 1472902297.1
Delta time: 111.778051376

So in this situation when I publish the TF from orbcamera_camera to world (ORBSLAM node) I have two secenarios;

• if I choose ros::Time::now() as timestap on Skimap side I get Lookup would require extrapolation into the past. Requested time 1472902308.293575041 but the earliest data is at time 1472902410.10, when looking up transform from frame [orbslam_camera] to frame [world].
• If I use insted msgRGB->header.stamp as timestamp I get Lookup would require extrapolation into the future. Requested time 1472902309.564114801 but the latest data is at time 1472902309.430578701, when looking up transform from frame [orbslam_camera] to frame [world].

Do you have any hint to fix this issue?

Thanks
Andrea

Hi, it seems that there is a slight issue in SkipList.hpp in the call NodeType *insert(K search_key, V new_value).

When search_key does not match curr_node->key a random level is selected, but the call randomlevel can return up to MAXLEVEL. When the code is compiled on Windows (with VS2017) it this behavior results in stack corruption by writing outside the range of update. Decrementing max return from randomlevel removes this issue.

Is this a bug that actually occurs on linux as well - but stays hidden because of more robust handling of stack corruptions? Or is it some portability issue. I have to admit that I'm a bit dense re the algorithm, so it is not entirely clear to me why the random level is chosen.

I am using opencv 3.2.0-dev

In file included from /home/rjn/catkin_ws/src/skimap_ros/include/slamdunk/camera_tracker.h:17:0,
                 from /home/rjn/catkin_ws/src/skimap_ros/src/slamdunk/feature_tracker.cpp:9:
/home/rjn/catkin_ws/src/skimap_ros/include/slamdunk/quadtree.h: In function ‘cv::Mat slamdunk::drawQuadTree(const QuadTreeT&, unsigned int)’:
/home/rjn/catkin_ws/src/skimap_ros/include/slamdunk/quadtree.h:332:5: error: ‘circle’ is not a member of ‘cv’
     cv::circle(img, cv::Point((it->m_coords.x() - tree.getOffset().x()) * imgsize / tree.getLength(), (it->m_coords.y() - tree.getOffset().y()) * imgsize / tree.getLength()),

and

In file included from /home/rjn/catkin_ws/src/skimap_ros/include/slamdunk/camera_tracker.h:20:0,
                 from /home/rjn/catkin_ws/src/skimap_ros/src/slamdunk/feature_tracker.cpp:9:
/usr/include/opencv2/nonfree/features2d.hpp: At global scope:
/usr/include/opencv2/nonfree/features2d.hpp:73:21: error: ‘vector’ has not been declared
                     vector<KeyPoint>& keypoints) const;
                     ^
/usr/include/opencv2/nonfree/features2d.hpp:73:27: error: expected ‘,’ or ‘...’ before ‘<’ token
                     vector<KeyPoint>& keypoints) const;
                           ^
/usr/include/opencv2/nonfree/features2d.hpp:77:21: error: ‘vector’ has not been declared
                     vector<KeyPoint>& keypoints,
                     ^
/usr/include/opencv2/nonfree/features2d.hpp:77:27: error: expected ‘,’ or ‘...’ before ‘<’ token
                     vector<KeyPoint>& keypoints,
                           ^
/usr/include/opencv2/nonfree/features2d.hpp:76:10: error: ‘void cv::SIFT::operator()(cv::InputArray, cv::InputArray, int) const’ cannot be overloaded
     void operator()(InputArray img, InputArray mask,

I have managed to implement your mapping solution with our SLAM system, however I have a question regarding the free space management used by the SkiMap.

When mapping areas with moving objects once the moving object is inserted into the SkiMap it is never filtered out, even when the same area is mapped without the object . By going through the code I was unable to find any ray casting implementation which would ensure that all voxels between the laser and the measured point are freed. Can you please confirm this and if so do you plan to release a version with this option, or do I have to implement it by myself. If this is the case, do you have any starting points?

Hi @m4nh ,
When printing the z coordinate of tiles2d I got 0.075. Shouldn't it be the agent height?
I am asking this because I wanted to filter the floor.
Thanks in advance,
Bruno

Hi,
the Mega cloud service that is hosting the bag file has a 2 GB data traffic limit per IP so is not possible download the file without paying the service.
Could you please share the bag file through another cloud service?
Tnks
Andrea

Hi @m4nh ,
I want to make a 2D grid visualization of occupied tiles using my SLAM mapping.
It is important to mention that there is a transformation of axis to match this SLAM reference frame:

• x-> y
• y -> z
• z->x

  for (int i = 0; i < tiles.size(); i++) {

  
    geometry_msgs::Point point;
    point.x = tiles[i].y;
    point.y = tiles[i].z;
    point.z = tiles[i].x;

 
    std_msgs::ColorRGBA color;
    if (tiles[i].data != NULL) {
      color.r = color.g = color.b =
          tiles[i].data->w >= min_voxel_weight_ ? 0.0 : 1.0;
      color.a = 1;
    } else {
      color.r = color.g = color.b = 1.0;
      color.a = 1;
    }

    voxels_marker.points.push_back(point);
    voxels_marker.colors.push_back(color);
  }

I changed the coordinates, although I obtain a wrong 2D grid.

Could you point me out what could be my issue?
Thanks a lot in advance,
Bruno
PS: The grey stripes are the 2D grid

Hi,
Thank you for your package.
I want to build the Skimap_ros on ubuntu 14.04 indigo, the errors are below

   [100%] Building CXX object skimap_ros/CMakeFiles/skimap_map_service.dir/src/nodes/skimap_map_service.cpp.o

[100%] Built target skimap_ros_generate_messages
/home/exbot/catkin_ws/src/skimap_ros/src/nodes/skimap_live.cpp: In function ‘int main(int, char**)’:
/home/exbot/catkin_ws/src/skimap_ros/src/nodes/skimap_live.cpp:554:108: error: no matching function for call to ‘ros::NodeHandle::param(const char [26], const char [11])’
std::string map_cloud_publisher_topic = nh->paramstd::string("map_cloud_publisher_topic","live_cloud");

Hi,

Thanks for your package.

I am having some issues generating the correct 3D map as you can see in the following image. Could you explain what do you think is the problem?

contents of the lunch file:

<arg name="camera" default="camera" />

<node name="skimap_live" output="screen" pkg="skimap_ros" type="skimap_live">
    
    <!-- Generic Parameters -->
    <param name="hz" value="10"/>

    <!-- Topics Parameters -->
    <param name="camera_rgb_topic" value="/$(arg camera)/rgb/image_raw"/>
    <param name="camera_depth_topic" value="/$(arg camera)/depth/image_raw"/>

<!-- Global and Camera TF Names -->
<param name="base_frame_name" value="odom"/>
<param name="camera_frame_name" value="camera_rgb_optical_frame"/>
    
    <!-- Cloud parameters  -->
    <param name="point_cloud_downscale" value="2"/>

    <!-- RGB-D Parameters -->
    <param name="fx" value="540.175979"/>
    <param name="fy" value="530.740137"/>
    <param name="cx" value="325.399891"/>
    <param name="cy" value="239.056878"/>
    <param name="rows" value="480"/>
    <param name="cols" value="640"/>
    <param name="camera_distance_max" value="3"/>
    <param name="camera_distance_min" value="0.45"/>
    
    <!-- Mapping parameters -->
    <param name="base_frame_name" value="odom"/>

    <param name="map_resolution" value="0.05"/>
    <param name="min_voxel_weight" value="100"/>
    <param name="ground_level" value="0.05"/>
    <param name="agent_height" value="1.5"/>
    <param name="enable_chisel" value="false"/>
    <param name="chisel_step" value="30"/>
    <param name="height_color" value="true"/>

</node>

<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find skimap_ros)/rviz/skimap_live.rviz" />

When skimap was integrated with ORBSLAM it used the skimap service. Is that to say ::

1-the service was called from ORBSLAM (could be called from ros_rgbd.cc)

2- that ORBSLAM only passed it tracking points or did it pass a full dense cloud.

3- If integrated using skimap service then publishing a tf or pose independently is still needed since skimap does not do this but only publishes the markers.

If depth map is not in meters but mm say 5000 to a meter.
Is there anyway for skip map to work with these metrics.

Hi @m4nh , Thanks for your excellent job, I want to try to reproduce it, but I failed,can you help me? I can only see the robot move in rviz. I can't get a correct maps.

Hi @m4nh ,
I have tried this package on my own slam system, but the 3d mapping result is not correct, there are so many vertical strips, the result is as following,

and the LiveCloud topic is this, it is only several narrow vertical strips:

But actually my depth image looks like the image below.

This is the skimap_live.launch that I have modified.
`

<arg name="camera" default="camera" />

<node name="skimap_live" output="screen" pkg="skimap_ros" type="skimap_live">

    <!-- Generic Parameters -->
    <param name="hz" value="10" />

    <!-- Topics Parameters -->
    <param name="camera_rgb_topic" value="/$(arg camera)/rgb/image_raw" />
    <param name="camera_depth_topic" value="/$(arg camera)/depth_registered/image_raw" />

    <!-- Cloud parameters  -->
    <param name="point_cloud_downscale" value="1" />

    <!-- RGB-D Parameters -->
    <param name="fx" value="520.5687" />
    <param name="fy" value="520.9826" />
    <param name="cx" value="328.6763" />
    <param name="cy" value="236.1047" />
    <param name="rows" value="480" />
    <param name="cols" value="640" />
    <param name="camera_distance_max" value="3" />
    <param name="camera_distance_min" value="0.45" />

    <!-- Mapping parameters -->
    <param name="base_frame_name" value="vodom" />
    <param name="camera_frame_name" value="camera_rgb_optical_frame" />
    <param name="map_resolution" value="0.05" />
    <param name="min_voxel_weight" value="100" />
    <param name="ground_level" value="0.05" />
    <param name="agent_height" value="1.5" />
    <param name="enable_chisel" value="false" />
    <param name="chisel_step" value="30" />
    <param name="height_color" value="true" />



</node>

<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find skimap_ros)/rviz/skimap_live.rviz" />

`
What's wrong with my operation, any suggestions? Thank you.

First of all, thank you very much for making this code public.

I have a question about Skimap functions. We are trying to build local map of the robot environment equipped with RGBD camera. For now we build octomap, then we extract the bounding box and afterwards add new point cloud to the last extracted bounding box. This process repeats continuously. Although the results are OK, the process is fairly slow and octomap structure does not allow for easy efficient implementation of this solution.

Could you please tell me if this is possible with Skimap. So, we want to build a map, crop map to remove everything outside a specified box and then update remaining box with new point cloud?

I've intergrated skimap with ptam.
I like this better because you do not have to do a mod since ptam published pose msg.
I have one issue: it works find when I feed it tiago_bar.bag but if I feed it a bag from other slam dataset
rgbd_dataset_freiburg3_long_office_household_validation.bag even when adjusted for camera parameters the point cloud and map are way off. see pictures below. Any idea why this would be. Is there different about the camera set up . I strip out all but the image and depth messages in both bag files.

My integrations launch file is as follows. note; ptam requires that you set the plane using its gui which will pop up once set it will forward pose.

<launch>
    
   
    <param name="use_sim_time" value="true" /> 


    <arg name="bag_file"         default="/home/rjn/data/bags/skimap_ros/new_tiago_bar.bag" />   
    <arg name="bag_image_topic"  default="/xtion/rgb/image_raw" />
    <arg name="bag_depth_topic"  default="/xtion/depth/image_raw" />    
    <arg name="bag_camera_frame" default="/xtion_rgb_optical_frame" />   
    <arg name="pose_base_frame"  default="/world" /> 
    <arg name="pose_topic"       default="/vslam/pose_world" />      <!-- "/vslam/pose" -->
 
 
     <!-- *** Make sue you update SkimapPtamFixParams to correct camera instrinces -->   
    <rosparam command="load" file="$(find skimap_ros)/SkimapPtamFixParams.yaml"/> 

    <node pkg="rosbag" type="play" name="player" output="screen" 
          args=" --clock --delay 15 -r 0.25 $(arg bag_file) /camera/rgb/image_color:=$(arg bag_image_topic)"/> 


    <!-- De-mosaics and undistorts the raw camera image stream. create mono gray image -->
    <node pkg="image_proc" type="image_proc" name="proc" ns="camera"> 
        <remap from="/camera/image_raw" to="$(arg bag_image_topic)"/>
    </node> 

    <!--   must be mono gray image -->
    <node name="ptam" pkg="ptam" type="ptam" clear_params="true" output="screen">
         <remap from="image"         to="/camera/image_mono" /> 
         <param name="parent_frame"  value="$(arg pose_base_frame)"/> 
         <rosparam command="load" file="$(find skimap_ros)/SkimapPtamFixParams.yaml"/>         
    </node>

    <node name="pose_tf" pkg="pose_tf" type="pose_tf" clear_params="true" output="screen">
         <param name="pose_topic"     value="$(arg pose_topic)"/>    
         <param name="world_frame"    value="$(arg pose_base_frame)"/>
         <param name="target_frame"   value="$(arg bag_camera_frame)"/>                  
    </node>
        
    <node name="skimap_live" output="screen" pkg="skimap_ros" type="skimap_live">
        <param name="camera_rgb_topic"   value="$(arg bag_image_topic)"/>
        <param name="camera_depth_topic" value="$(arg bag_depth_topic)"/>
        <param name="base_frame_name"    value="$(arg pose_base_frame)"/>
        <param name="camera_frame_name"  value="$(arg bag_camera_frame)"/>   
        <rosparam command="load" file="$(find skimap_ros)/SkimapPtamFixParams.yaml"/>     

    </node>


    <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find skimap_ros)/rviz/skimap_live_orb.rviz" />

</launch>

pose_tf.cpp : code for node

pose_tf node is as follows:

#include <ros/ros.h>
#include <ros/console.h>
#include <tf/transform_broadcaster.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"

std::string pose_topic   = "                      ";
std::string world_frame  = "                      ";
std::string target_frame = "                      ";

void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
    tf::TransformBroadcaster br;
    tf::Transform transform;

    geometry_msgs::Pose pose = msg->pose.pose;
    geometry_msgs::Point position = pose.position;
    geometry_msgs::Quaternion orientation = pose.orientation;

    transform.setOrigin( tf::Vector3(position.x, position.y, position.z) );
    transform.setRotation( tf::Quaternion( orientation.x, orientation.y, orientation.z, orientation.w) );  
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), world_frame, target_frame));
}


int main(int argc, char** argv){
    ros::init(argc, argv, "pose_tf_broadcaster");
    ROS_INFO("Broadcasted transformation pose_tf_broadcaster");
    ros::NodeHandle nh;
    nh.param<std::string>("pose_topic",   pose_topic,  "/xxxxx/pose"); 
    std::cout <<   pose_topic << std::endl;
    std::cout <<   pose_topic << std::endl;   
    nh.param<std::string>("world_frame",  world_frame,  "/xxxx/world");   
    nh.param<std::string>("target_frame", target_frame, "/xxxx/openni_rgb_optical_frame");          
    ros::Subscriber sub = nh.subscribe(pose_topic, 1000, poseCallback);

    ros::spin();

    return 0;
}

I combined the Parameter files as follows:

# ----------------------------------------------------------------------
#          PTAM: beware of duplicate params in SKIPMAP below
# ----------------------------------------------------------------------

gui: True

WiggleScale: 0.1
BundleMEstimator: Tukey
TrackerMEstimator: Tukey
MinTukeySigma: 0.4
CandidateMinSTScore: 70  
Calibrator_BlurSigma: 1.0
Calibrator_MeanGate: 10
Calibrator_MinCornersForGrabbedImage: 20
Calibrator_Optimize: 0
Calibrator_Show: 1
Calibrator_NoDistortion: 0
CameraCalibrator_MaxStepDistFraction: 0.3
CameraCalibrator_CornerPatchSize: 20
GLWindowMenu_Enable: True
GLWindowMenu_mgvnMenuItemWidth: 90
GLWindowMenu_mgvnMenuTextOffset: 20
InitLevel: 1
MaxKFDistWiggleMult: 1
MaxPatchesPerFrame: 300
MaxKF: 15
TrackingQualityFoundPixels: 50
UseKFPixelDist: True
NoLevelZeroMapPoints: True
FASTMethod: OAST16
MaxStereoInitLoops: 4
AutoInitPixel: 20

# from ATANCamera.cc
#  mvFocal[0] = mvImageSize[0] * mgvvCameraParams[0];
#  mvFocal[1] = mvImageSize[1] * mgvvCameraParams[1];
#  mvCenter[0] = mvImageSize[0] * mgvvCameraParams[2] - 0.5;
#  mvCenter[1] = mvImageSize[1] * mgvvCameraParams[3] - 0.5;
#                           fx	    fy	    cx	    cy
#         Freiburg 3 RGB	535.4	539.2	320.1	247.6

ImageSizeX: 640
ImageSizeY: 480
ARBuffer_width: 1200
ARBuffer_height: 900

Cam_fx: 0.836562  # 535.4/640 
Cam_fy: 1.1233333   # 539.2/480 
Cam_cx: 0.500156    # 320.1/640
Cam_cy: 0.515833    # 247.6/480 
Cam_s:  0.25         # radial distortion

# ----------------------------------------------------------------------
#          SKIP MAP
# ----------------------------------------------------------------------

# RGB-D Parameters -->
rows: 480  
cols: 640  
fx: 535.4 
fy: 539.2 
cx: 320.1  
cy: 247.6  


 # Generic Parameters -->
hz: 10 
              
# Cloud parameters  -->
#point_cloud_downscale: 2  
       
# Mapping parameters -->       
mapping: true
map_resolution: 0.01
camera_distance_max: 8
#camera_distance_min: 0.45
max_integration_step: 1  
max_deintegration_step: 5 

When I try launch slamdunk_tracker.launch, I have got error:

cannot launch node of type [skimap_ros/slamdunk_tracker]: can't locate node [slamdunk_tracker] in package [skimap_ros]

how can I fix that?

when I am using this package, I find that there is no file called <skimap_ros/SkimapIntegrationService.h>, but it is included in one head file.
So I don't know where to find it.

Hi @m4nh ,
When I use other dataset to run the skimap_ros, this issue is occoured in rviz. You have said that we should launch a TF LIstener in the skimap node,but I cannot know how to do that . Can you describe how to do it in detail ?
Thank you very much .

I see it referenced in integration_service_callback and the service client but i cannot find it. Do you have it?

I am running the taigo_bar bag file in two ways.
First as given and everything works fine....I publish the pose from world to the optical frame. Map looks good all very good.

Then I have the same bag file where I removed all messages except for the depth and image messages . But when I run against it the map is all over the place and does not appear correct. This is not just a rotation issue...the map is clearly wrong.

Why would that happen. Why is the rgb image, depth image and pose not enough to correctly build the map? Does the depth and image have different poses? I am stuck on this results. If you can help me out with an explanation that would great...maybe hint to how to work this issue.

Hi,
I was trying to understand how to iterate over map grid and get cell occupancy, but I couldn't understand from SkiMap header file. Could you point me out how can i proceed?
Thanks a lot.
Regards,
Bruno

Hi @m4nh , Thank you for your sharing, it is amazing work. I have run this successful with ORB SLAM, but I have some issues about how to save the 3D map. Is there have any function can be used to save the final map? And, I found it will make the computer freeze up when I using TUM RGBD dataset for 3D mapping, the CPU of my computer is I7-8700K with 32G RAM and RTX2070. So, is there any way to solve this problem?

Hi! I'm trying to slamdunk_tracker.launch,I run this :

1. roslaunch skimap_ros slamdunk_tracker.launch
2. rosbag play tiago_lar.bag --clock
   However, I got the error :1、"Fixed Frame [slam_map] does not exist" on the RVIZ
   2、“ERROR: cannot launch node of type [skimap_ros/slamdunk_tracker]: can't locate node [slamdunk_tracker] in package [skimap_ros]”after the slamdunk_tracker.launch
   How can I solve this problem?thx
   i catkin_make the ws is not error

I am trying to use SkiMap on my Nvidia Jetson TX2 board to create a map using the zed camera. I modified the launch file skimap_live.launch according to my needs, however, when I run the launch file, I get the following error:

nvidia@tegra-ubuntu:/data/code/fount_landing/fount_ws/src/fount_slam/launch$ roslaunch zed_skimap_live_test.launch 
... logging to /home/nvidia/.ros/log/ab5e87d2-3fef-11e8-8608-00044b8d1201/roslaunch-tegra-ubuntu-13970.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://tegra-ubuntu:41535/

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.13
 * /skimap_live/agent_height: 5
 * /skimap_live/base_frame_name: odom
 * /skimap_live/camera_depth_topic: /depth/depth_regi...
 * /skimap_live/camera_frame_name: zed_left_camera
 * /skimap_live/camera_rgb_topic: /left/image_rect_...
 * /skimap_live/chisel_step: 30
 * /skimap_live/enable_chisel: False
 * /skimap_live/ground_level: 1.5
 * /skimap_live/height_color: True
 * /skimap_live/hz: 0.5
 * /skimap_live/map_resolution: 0.02
 * /skimap_live/min_voxel_weight: 10
 * /skimap_live/point_cloud_downscale: 1

NODES
  /
    rviz (rviz/rviz)
    skimap_live (skimap_ros/skimap_live)

ROS_MASTER_URI=http://localhost:11311

process[skimap_live-1]: started with pid [14014]
process[rviz-2]: started with pid [14015]
[ INFO] [1523715936.304001826]: rviz version 1.12.15
[ INFO] [1523715936.304148546]: compiled against Qt version 5.5.1
[ INFO] [1523715936.304220354]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1523715937.294223684]: Stereo is NOT SUPPORTED
[ INFO] [1523715937.294670243]: OpenGl version: 4.5 (GLSL 4.5).
*** stack smashing detected ***: /data/code/fount_landing/fount_ws/devel/lib/skimap_ros/skimap_live terminated
[skimap_live-1] process has died [pid 14014, exit code -6, cmd /data/code/fount_landing/fount_ws/devel/lib/skimap_ros/skimap_live __name:=skimap_live __log:=/home/nvidia/.ros/log/ab5e87d2-3fef-11e8-8608-00044b8d1201/skimap_live-1.log].
log file: /home/nvidia/.ros/log/ab5e87d2-3fef-11e8-8608-00044b8d1201/skimap_live-1*.log

It would be great if someone could help me out in identifying the possible sources for this error and plausible solutions for it.