search

felixendres / rgbdslam_v2

RGB-D SLAM for ROS
GNU General Public License v3.0
940 stars 400 forks source link

Failed to process Kinect2 PointCloud Data #18

Closed aginika closed 8 years ago

aginika commented 8 years ago

When I try to use with Kinect2, the below error appeared. How can I solve this problem?

rgbdslam: /home/aginika/ros/indigo/src/rgbdslam/src/node.cpp:315: Node::Node(cv::Mat, cv::Ptr<cv::FeatureDetector>, cv::Ptr<cv::DescriptorExtractor>, 
pcl::PointCloud<pcl::PointXYZRGB>::Ptr, cv::Mat): Assertion `feature_locations_2d_.size() == feature_locations_3d_.size()' failed

the whole output when died is below

$ roslaunch test_rgbd rgbd_slam.launch 
... logging to /home/aginika/.ros/log/f2d484b2-8161-11e5-8579-40a8f055f334/roslaunch-remi-28623.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://remi:39140/

SUMMARY
========

PARAMETERS
 * /rgbdslam/config/backend_solver: pcg
 * /rgbdslam/config/base_frame_name: /kinect2_head_rgb...
 * /rgbdslam/config/camera_info_topic: /kinect2_head/qhd...
 * /rgbdslam/config/cloud_creation_skip_step: 2
 * /rgbdslam/config/cloud_display_type: POINTS
 * /rgbdslam/config/detector_grid_resolution: 3
 * /rgbdslam/config/feature_detector_type: ORB
 * /rgbdslam/config/feature_extractor_type: ORB
 * /rgbdslam/config/max_keypoints: 600
 * /rgbdslam/config/max_matches: 300
 * /rgbdslam/config/min_sampled_candidates: 4
 * /rgbdslam/config/neighbor_candidates: 4
 * /rgbdslam/config/optimizer_skip_step: 1
 * /rgbdslam/config/pose_relative_to: largest_loop
 * /rgbdslam/config/predecessor_candidates: 4
 * /rgbdslam/config/ransac_iterations: 100
 * /rgbdslam/config/topic_image_depth: /kinect2_head/qhd...
 * /rgbdslam/config/topic_image_mono: /kinect2_head/qhd...
 * /rgbdslam/config/topic_points: /kinect2_head/qhd...
 * /rosdistro: indigo
 * /rosversion: 1.11.13

NODES
  /
    rgbdslam (rgbdslam/rgbdslam)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[rgbdslam-1]: started with pid [28641]
Initializing Node...
[ INFO] [1446471301.608257969]: Connected to roscore
[ INFO] [1446471301.783546557]: Using ORB keypoint detector.
[ INFO] [1446471301.783630228]: Using gridded keypoint detector with 3x3 cells, keeping 900 keypoints in total.
[ INFO] [1446471301.783662565]: Using adjusted keypoint detector with 5 maximum iterations, keeping the number of keypoints between 67 and 100
[ INFO] [1446471301.799234774]: Listening to /kinect2_head/qhd/image_color_rect, /kinect2_head/qhd/image_depth_rect and /kinect2_head/qhd/points
[ WARN] [1446471302.504091410]: First RGBD-Data Received
[ INFO] [1446471302.525659170]: Construction of Node with ORB Features
[ INFO] [1446471302.549767034]: Feature 2d size: 850, 3D: 600
[ INFO] [1446471302.549834571]: Feature 2d size: 600, 3D: 600
rgbdslam: /home/aginika/ros/indigo/src/rgbdslam/src/node.cpp:315: Node::Node(cv::Mat, cv::Ptr<cv::FeatureDetector>, cv::Ptr<cv::DescriptorExtractor>, pcl::PointCloud<pcl::PointXYZRGB>::Ptr, cv::Mat): Assertion `feature_locations_2d_.size() == feature_locations_3d_.size()' failed.
================================================================================REQUIRED process [rgbdslam-1] has died!
process has died [pid 28641, exit code -6, cmd /home/aginika/ros/indigo/devel/lib/rgbdslam/rgbdslam __name:=rgbdslam __log:=/home/aginika/.ros/log/f2d484b2-8161-11e5-8579-40a8f055f334/rgbdslam-1.log].
log file: /home/aginika/.ros/log/f2d484b2-8161-11e5-8579-40a8f055f334/rgbdslam-1*.log
Initiating shutdown!
================================================================================
[rgbdslam-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

I am using iai_kinect2(https://github.com/code-iai/iai_kinect2) as a launcher of Kinect2 and I passed the topics to below launch file. When I use this with xtion, it works well.

<launch>
  <node pkg="rgbdslam" type="rgbdslam" name="rgbdslam" cwd="node" required="true" output="screen">
    <!-- Input data settings-->
    <param name="config/topic_image_mono"              value="/kinect2_head/qhd/image_color_rect"/>
    <param name="config/topic_image_depth"             value="/kinect2_head/qhd/image_depth_rect"/>
    <param name="config/camera_info_topic"             value="/kinect2_head/qhd/camera_info"/>

    <param name="config/base_frame_name"             value="/kinect2_head_rgb_optical_frame"/>

    <!-- <param name="config/topic_image_mono"              value="/kinect_head/rgb/image_color"/>  -->
    <!-- <param name="config/topic_image_depth"             value="/kinect_head/depth_registered/hw_registered/image_rect_raw"/>  -->
    <!-- <param name="config/camera_info_topic"             value="/kinect_head/rgb/camera_info"/> -->

    <!-- <param name="config/base_frame_name"             value="/kinect_head_rgb_optical_frame"/> -->

    <param name="config/topic_points"                  value="/kinect2_head/qhd/points"/> <!--if empty, poincloud will be reconstructed from image and depth -->

    <!-- These are the default values of some important parameters -->
    <param name="config/feature_extractor_type"        value="ORB"/><!-- also available: SIFT, SIFTGPU, SURF, SURF128 (extended SURF), ORB. -->
    <param name="config/feature_detector_type"         value="ORB"/><!-- also available: SIFT, SURF, GFTT (good features to track), ORB. -->
    <param name="config/detector_grid_resolution"      value="3"/><!-- detect on a 3x3 grid (to spread ORB keypoints and parallelize SIFT and SURF) -->
    <param name="config/max_keypoints"                 value="600"/><!-- Extract no more than this many keypoints -->
    <param name="config/max_matches"                   value="300"/><!-- Keep the best n matches (important for ORB to set lower than max_keypoints) -->

    <param name="config/min_sampled_candidates"        value="4"/><!-- Frame-to-frame comparisons to random frames (big loop closures) -->
    <param name="config/predecessor_candidates"        value="4"/><!-- Frame-to-frame comparisons to sequential frames-->
    <param name="config/neighbor_candidates"           value="4"/><!-- Frame-to-frame comparisons to graph neighbor frames-->
    <param name="config/ransac_iterations"             value="100"/>
    <param name="config/cloud_creation_skip_step"      value="2"/><!-- subsample the images' pixels (in both, width and height), when creating the cloud (and therefore reduce memory consumption) -->

    <param name="config/cloud_display_type"            value="POINTS"/><!-- Show pointclouds as points (as opposed to TRIANGLE_STRIP) -->
    <param name="config/pose_relative_to"              value="largest_loop"/><!-- optimize only a subset of the graph: "largest_loop" = Everything from the earliest matched frame to the current one. Use "first" to optimize the full graph, "inaffected" to optimize only the fra\
mes that were matched (not those inbetween for loops) -->
    <param name="config/backend_solver"                value="pcg"/><!-- pcg is faster and good for continuous online optimization, cholmod and csparse are better for offline optimization (without good initial guess)-->
    <param name="config/optimizer_skip_step"           value="1"/><!-- optimize only every n-th frame -->
  </node>
</launch>

Each topic's height x width was all 540 x 960.

$ rostopic echo --noarr /kinect2_head/qhd/image_depth_rect
header: 
  seq: 228
  stamp: 
    secs: 1446471841
    nsecs: 146858714
  frame_id: kinect2_head_rgb_optical_frame
height: 540
width: 960
encoding: 16UC1
is_bigendian: 0
step: 1920

---

This is tf tree. frames

Thank you.

kbalisciano commented 8 years ago

I would modify the V2 driver to output to the same topic as the original kinect.

<param name="config/topic_image_mono" value="/kinect2_head/qhd/image_color_rect"/> <param name="config/topic_image_depth" value="/kinect2_head/qhd/image_depth_rect"/> <param name="config/camera_info_topic" value="/kinect2_head/qhd/camera_info"/>

<param name="config/base_frame_name"

value="/kinect2_head_rgb_optical_frame"/>

<!-- <param name="config/topic_image_mono"

value="/kinect_head/rgb/image_color"/> -->

<!-- <param name="config/camera_info_topic"

value="/kinect_head/rgb/camera_info"/>

See the image_color vs image_color_rect.

Also on the kinect V2 is the depth topic now image_depth_rect? Sorry I don't use the V2 but it seems like you're not passing it a point cloud

On Mon, Nov 2, 2015 at 8:48 AM, Yuto Inagaki notifications@github.com wrote:

When I try to use with Kinect2, the below error appeared. How can I solve this problem?

rgbdslam: /home/aginika/ros/indigo/src/rgbdslam/src/node.cpp:315: Node::Node(cv::Mat, cv::Ptrcv::FeatureDetector, cv::Ptrcv::DescriptorExtractor, pcl::PointCloudpcl::PointXYZRGB::Ptr, cv::Mat): Assertion `feature_locations2d.size() == feature_locations3d.size()' failed

the whole output when died is below

$ roslaunch test_rgbd rgbd_slam.launch ... logging to /home/aginika/.ros/log/f2d484b2-8161-11e5-8579-40a8f055f334/roslaunch-remi-28623.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://remi:39140/

SUMMARY

PARAMETERS

  • /rgbdslam/config/backend_solver: pcg
  • /rgbdslam/config/base_frame_name: /kinect2_head_rgb...
  • /rgbdslam/config/camera_info_topic: /kinect2_head/qhd...
  • /rgbdslam/config/cloud_creation_skip_step: 2
  • /rgbdslam/config/cloud_display_type: POINTS
  • /rgbdslam/config/detector_grid_resolution: 3
  • /rgbdslam/config/feature_detector_type: ORB
  • /rgbdslam/config/feature_extractor_type: ORB
  • /rgbdslam/config/max_keypoints: 600
  • /rgbdslam/config/max_matches: 300
  • /rgbdslam/config/min_sampled_candidates: 4
  • /rgbdslam/config/neighbor_candidates: 4
  • /rgbdslam/config/optimizer_skip_step: 1
  • /rgbdslam/config/pose_relative_to: largest_loop
  • /rgbdslam/config/predecessor_candidates: 4
  • /rgbdslam/config/ransac_iterations: 100
  • /rgbdslam/config/topic_image_depth: /kinect2_head/qhd...
  • /rgbdslam/config/topic_image_mono: /kinect2_head/qhd...
  • /rgbdslam/config/topic_points: /kinect2_head/qhd...
  • /rosdistro: indigo
  • /rosversion: 1.11.13

NODES / rgbdslam (rgbdslam/rgbdslam)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found process[rgbdslam-1]: started with pid [28641] Initializing Node... [ INFO] [1446471301.608257969]: Connected to roscore [ INFO] [1446471301.783546557]: Using ORB keypoint detector. [ INFO] [1446471301.783630228]: Using gridded keypoint detector with 3x3 cells, keeping 900 keypoints in total. [ INFO] [1446471301.783662565]: Using adjusted keypoint detector with 5 maximum iterations, keeping the number of keypoints between 67 and 100 [ INFO] [1446471301.799234774]: Listening to /kinect2_head/qhd/image_color_rect, /kinect2_head/qhd/image_depth_rect and /kinect2_head/qhd/points [ WARN] [1446471302.504091410]: First RGBD-Data Received [ INFO] [1446471302.525659170]: Construction of Node with ORB Features [ INFO] [1446471302.549767034]: Feature 2d size: 850, 3D: 600 [ INFO] [1446471302.549834571]: Feature 2d size: 600, 3D: 600 rgbdslam: /home/aginika/ros/indigo/src/rgbdslam/src/node.cpp:315: Node::Node(cv::Mat, cv::Ptrcv::FeatureDetector, cv::Ptrcv::DescriptorExtractor, pcl::PointCloudpcl::PointXYZRGB::Ptr, cv::Mat): Assertion `feature_locations2d.size() == feature_locations3d.size()' failed. ================================================================================REQUIRED process [rgbdslam-1] has died! process has died [pid 28641, exit code -6, cmd /home/aginika/ros/indigo/devel/lib/rgbdslam/rgbdslam name:=rgbdslam log:=/home/aginika/.ros/log/f2d484b2-8161-11e5-8579-40a8f055f334/rgbdslam-1.log]. log file: /home/aginika/.ros/log/f2d484b2-8161-11e5-8579-40a8f055f334/rgbdslam-1*.log

Initiating shutdown!

[rgbdslam-1] killing on exit shutting down processing monitor... ... shutting down processing monitor complete done

I am using iai_kinect2(https://github.com/code-iai/iai_kinect2) as a launcher of Kinect2 and I passed the topics to below launch file. When I use this with xtion, it works well.

```

Each topic's height x width was all 540 x 960.

$ rostopic echo --noarr /kinect2_head/qhd/image_depth_rect header: seq: 228 stamp: secs: 1446471841 nsecs: 146858714 frame_id: kinect2_head_rgb_optical_frame height: 540 width: 960 encoding: 16UC1 is_bigendian: 0

step: 1920

This is tf tree. [image: frames] https://cloud.githubusercontent.com/assets/3803922/10883221/cc69a846-81b3-11e5-8d16-654f0efbc843.png

Thank you.

— Reply to this email directly or view it on GitHub https://github.com/felixendres/rgbdslam_v2/issues/18.

aginika commented 8 years ago

Thank you for your advise! I will try it or use v1

felixendres commented 8 years ago

I'd guess the problem lies with the dimensions of the cloud not fitting the dimension of the input images. You can try omitting the cloud, i.e. set the "topic_points" to an empty string (""). Then, rgbdslam will compute the 3D position of the features using the depth image for reprojection instead of taking the 3D info from the cloud.