From f8b08cd019c40bf8200b91ef43c15a5c9c7b2f05 Mon Sep 17 00:00:00 2001 From: Erik Rosen Date: Thu, 14 Nov 2013 17:02:41 +0000 Subject: [PATCH 01/11] publishes odom covariance --- ccny_rgbd/src/apps/visual_odometry.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ccny_rgbd/src/apps/visual_odometry.cpp b/ccny_rgbd/src/apps/visual_odometry.cpp index cad34a2..d5abc77 100644 --- a/ccny_rgbd/src/apps/visual_odometry.cpp +++ b/ccny_rgbd/src/apps/visual_odometry.cpp @@ -362,6 +362,13 @@ void VisualOdometry::publishOdom(const std_msgs::Header& header) OdomMsg odom; odom.header.stamp = header.stamp; odom.header.frame_id = fixed_frame_; + boost::array pose_covariance = {{0.0015, 0, 0, 0, 0, 0, + 0, 0.0015, 0, 0, 0, 0, + 0, 0, 1e-6 , 0, 0, 0, + 0, 0, 0, 1e-6 , 0, 0, + 0, 0, 0, 0, 1e-6 , 0, + 0, 0, 0, 0, 0, 0.05}}; + odom.pose.covariance=pose_covariance; tf::poseTFToMsg(f2b_, odom.pose.pose); odom_publisher_.publish(odom); } From e58ec7209632fd87e3d95410a64ff58d0d1ed72f Mon Sep 17 00:00:00 2001 From: Erik Rosen Date: Thu, 14 Nov 2013 18:33:41 +0000 Subject: [PATCH 02/11] added covariance parameter loading --- .../include/ccny_rgbd/apps/visual_odometry.h | 10 ++++++++ ccny_rgbd/src/apps/visual_odometry.cpp | 25 ++++++++++++++----- 2 files changed, 29 insertions(+), 6 deletions(-) diff --git a/ccny_rgbd/include/ccny_rgbd/apps/visual_odometry.h b/ccny_rgbd/include/ccny_rgbd/apps/visual_odometry.h index 984b62f..7c6fc5f 100644 --- a/ccny_rgbd/include/ccny_rgbd/apps/visual_odometry.h +++ b/ccny_rgbd/include/ccny_rgbd/apps/visual_odometry.h @@ -126,6 +126,16 @@ class VisualOdometry bool publish_model_cloud_; bool publish_model_cov_; + /** + * This is the pose covariance matrix + */ + double xx; + double yy; + double zz; + double yawyaw; + double pitchpitch; + double rollroll; + /** @brief Feature detector type parameter * * Possible values: diff --git a/ccny_rgbd/src/apps/visual_odometry.cpp b/ccny_rgbd/src/apps/visual_odometry.cpp index d5abc77..07481a3 100644 --- a/ccny_rgbd/src/apps/visual_odometry.cpp +++ b/ccny_rgbd/src/apps/visual_odometry.cpp @@ -101,6 +101,19 @@ void VisualOdometry::initParams() if (!nh_private_.getParam ("queue_size", queue_size_)) queue_size_ = 5; + if (!nh_private_.getParam ("x_variance", xx)) + xx = 0.3; + if (!nh_private_.getParam ("y_variance", yy)) + yy = 0.3; + if (!nh_private_.getParam ("z_variance", zz)) + zz = 0.3; + if (!nh_private_.getParam ("yaw_variance", yawyaw)) + yawyaw = 0.3; + if (!nh_private_.getParam ("pitch_variance", pitchpitch)) + pitchpitch = 0.3; + if (!nh_private_.getParam ("roll_variance", rollroll)) + rollroll = 0.3; + // detector params if (!nh_private_.getParam ("feature/publish_feature_cloud", publish_feature_cloud_)) @@ -362,12 +375,12 @@ void VisualOdometry::publishOdom(const std_msgs::Header& header) OdomMsg odom; odom.header.stamp = header.stamp; odom.header.frame_id = fixed_frame_; - boost::array pose_covariance = {{0.0015, 0, 0, 0, 0, 0, - 0, 0.0015, 0, 0, 0, 0, - 0, 0, 1e-6 , 0, 0, 0, - 0, 0, 0, 1e-6 , 0, 0, - 0, 0, 0, 0, 1e-6 , 0, - 0, 0, 0, 0, 0, 0.05}}; + boost::array pose_covariance = {{xx, 0, 0, 0, 0, 0, + 0, yy, 0, 0, 0, 0, + 0, 0, zz , 0, 0, 0, + 0, 0, 0, yawyaw , 0, 0, + 0, 0, 0, 0, pitchpitch , 0, + 0, 0, 0, 0, rollroll}}; odom.pose.covariance=pose_covariance; tf::poseTFToMsg(f2b_, odom.pose.pose); odom_publisher_.publish(odom); From eea458f729ecb2057c8479f7d5b8be64496590a5 Mon Sep 17 00:00:00 2001 From: Erik Rosen Date: Thu, 14 Nov 2013 18:52:44 +0000 Subject: [PATCH 03/11] publishes odom covariance --- ccny_rgbd/src/apps/visual_odometry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ccny_rgbd/src/apps/visual_odometry.cpp b/ccny_rgbd/src/apps/visual_odometry.cpp index 07481a3..56f7e97 100644 --- a/ccny_rgbd/src/apps/visual_odometry.cpp +++ b/ccny_rgbd/src/apps/visual_odometry.cpp @@ -380,7 +380,7 @@ void VisualOdometry::publishOdom(const std_msgs::Header& header) 0, 0, zz , 0, 0, 0, 0, 0, 0, yawyaw , 0, 0, 0, 0, 0, 0, pitchpitch , 0, - 0, 0, 0, 0, rollroll}}; + 0, 0, 0, 0, 0, rollroll}}; odom.pose.covariance=pose_covariance; tf::poseTFToMsg(f2b_, odom.pose.pose); odom_publisher_.publish(odom); From 5715f274b49b71f90b1ff3ea04b0bbf9c86ae56f Mon Sep 17 00:00:00 2001 From: Erik Rosen Date: Mon, 20 Jan 2014 22:50:10 +0000 Subject: [PATCH 04/11] graphSLAM enabled in realtime --- .../include/ccny_rgbd/apps/keyframe_mapper.h | 9 +- ccny_rgbd/launch/visual_odometry.launch | 2 +- ccny_rgbd/src/apps/keyframe_mapper.cpp | 72 ++++- .../ccny_rgbd/cfg/FeatureDetectorConfig.py | 74 ++++++ .../src/ccny_rgbd/cfg/GftDetectorConfig.py | 70 +++++ .../src/ccny_rgbd/cfg/OrbDetectorConfig.py | 70 +++++ .../src/ccny_rgbd/cfg/RGBDImageProcConfig.py | 70 +++++ .../src/ccny_rgbd/cfg/StarDetectorConfig.py | 70 +++++ .../src/ccny_rgbd/cfg/SurfDetectorConfig.py | 70 +++++ .../src/ccny_rgbd/srv/_AddManualKeyframe.py | 176 ++++++++++++ ccny_rgbd/src/ccny_rgbd/srv/_GenerateGraph.py | 176 ++++++++++++ ccny_rgbd/src/ccny_rgbd/srv/_Load.py | 210 +++++++++++++++ .../src/ccny_rgbd/srv/_PublishKeyframe.py | 189 +++++++++++++ .../src/ccny_rgbd/srv/_PublishKeyframes.py | 210 +++++++++++++++ ccny_rgbd/src/ccny_rgbd/srv/_Save.py | 210 +++++++++++++++ ccny_rgbd/src/ccny_rgbd/srv/_SolveGraph.py | 176 ++++++++++++ .../rgbdtools/features/feature_detector.h | 121 +++++++++ .../include/rgbdtools/features/gft_detector.h | 77 ++++++ .../include/rgbdtools/features/orb_detector.h | 78 ++++++ .../rgbdtools/features/star_detector.h | 77 ++++++ .../rgbdtools/features/surf_detector.h | 74 ++++++ .../rgbdtools/graph/keyframe_association.h | 77 ++++++ .../rgbdtools/graph/keyframe_graph_detector.h | 251 ++++++++++++++++++ .../rgbdtools/graph/keyframe_graph_solver.h | 72 +++++ .../graph/keyframe_graph_solver_g2o.h | 98 +++++++ lib_rgbdtools/include/rgbdtools/header.h | 19 ++ lib_rgbdtools/include/rgbdtools/map_util.h | 142 ++++++++++ .../registration/motion_estimation.h | 111 ++++++++ .../motion_estimation_icp_prob_model.h | 238 +++++++++++++++++ lib_rgbdtools/include/rgbdtools/rgbd_frame.h | 203 ++++++++++++++ .../include/rgbdtools/rgbd_keyframe.h | 122 +++++++++ lib_rgbdtools/include/rgbdtools/rgbd_util.h | 217 +++++++++++++++ lib_rgbdtools/include/rgbdtools/rgbdtools.h | 49 ++++ lib_rgbdtools/include/rgbdtools/types.h | 72 +++++ lib_rgbdtools/rgbdtools | 0 lib_rgbdtools/rgbdtools_git | 1 + lib_rgbdtools/rospack_nosubdirs | 0 37 files changed, 3947 insertions(+), 6 deletions(-) create mode 100644 ccny_rgbd/src/ccny_rgbd/cfg/FeatureDetectorConfig.py create mode 100644 ccny_rgbd/src/ccny_rgbd/cfg/GftDetectorConfig.py create mode 100644 ccny_rgbd/src/ccny_rgbd/cfg/OrbDetectorConfig.py create mode 100644 ccny_rgbd/src/ccny_rgbd/cfg/RGBDImageProcConfig.py create mode 100644 ccny_rgbd/src/ccny_rgbd/cfg/StarDetectorConfig.py create mode 100644 ccny_rgbd/src/ccny_rgbd/cfg/SurfDetectorConfig.py create mode 100644 ccny_rgbd/src/ccny_rgbd/srv/_AddManualKeyframe.py create mode 100644 ccny_rgbd/src/ccny_rgbd/srv/_GenerateGraph.py create mode 100644 ccny_rgbd/src/ccny_rgbd/srv/_Load.py create mode 100644 ccny_rgbd/src/ccny_rgbd/srv/_PublishKeyframe.py create mode 100644 ccny_rgbd/src/ccny_rgbd/srv/_PublishKeyframes.py create mode 100644 ccny_rgbd/src/ccny_rgbd/srv/_Save.py create mode 100644 ccny_rgbd/src/ccny_rgbd/srv/_SolveGraph.py create mode 100644 lib_rgbdtools/include/rgbdtools/features/feature_detector.h create mode 100644 lib_rgbdtools/include/rgbdtools/features/gft_detector.h create mode 100644 lib_rgbdtools/include/rgbdtools/features/orb_detector.h create mode 100644 lib_rgbdtools/include/rgbdtools/features/star_detector.h create mode 100644 lib_rgbdtools/include/rgbdtools/features/surf_detector.h create mode 100644 lib_rgbdtools/include/rgbdtools/graph/keyframe_association.h create mode 100644 lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_detector.h create mode 100644 lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver.h create mode 100644 lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver_g2o.h create mode 100644 lib_rgbdtools/include/rgbdtools/header.h create mode 100644 lib_rgbdtools/include/rgbdtools/map_util.h create mode 100644 lib_rgbdtools/include/rgbdtools/registration/motion_estimation.h create mode 100644 lib_rgbdtools/include/rgbdtools/registration/motion_estimation_icp_prob_model.h create mode 100644 lib_rgbdtools/include/rgbdtools/rgbd_frame.h create mode 100644 lib_rgbdtools/include/rgbdtools/rgbd_keyframe.h create mode 100644 lib_rgbdtools/include/rgbdtools/rgbd_util.h create mode 100644 lib_rgbdtools/include/rgbdtools/rgbdtools.h create mode 100644 lib_rgbdtools/include/rgbdtools/types.h create mode 100644 lib_rgbdtools/rgbdtools create mode 160000 lib_rgbdtools/rgbdtools_git create mode 100644 lib_rgbdtools/rospack_nosubdirs diff --git a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h index daed3c0..fb829fa 100644 --- a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h +++ b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h @@ -204,6 +204,7 @@ class KeyframeMapper ros::Publisher poses_pub_; ///< ROS publisher for the keyframe poses ros::Publisher kf_assoc_pub_; ///< ROS publisher for the keyframe associations ros::Publisher path_pub_; ///< ROS publisher for the keyframe path + ros::Publisher pose_correction_pub_; ///< ROS publisher for the pose correction transform /** @brief ROS service to generate the graph correpondences */ ros::ServiceServer generate_graph_service_; @@ -257,7 +258,8 @@ class KeyframeMapper double octomap_res_; ///< tree resolution for octomap (in meters) double kf_dist_eps_; ///< linear distance threshold between keyframes double kf_angle_eps_; ///< angular distance threshold between keyframes - bool octomap_with_color_; ///< whetehr to save Octomaps with color info + bool octomap_with_color_; ///< whetehr to save Octomaps with color info + bool online_graph_opt_; //Whether or not to do online graph optimization and association checking double max_map_z_; ///< maximum z (in fixed frame) when exporting maps. // state vars @@ -269,8 +271,11 @@ class KeyframeMapper rgbdtools::KeyframeGraphSolverG2O graph_solver_; ///< optimizes the graph for global alignement rgbdtools::KeyframeAssociationVector associations_; ///< keyframe associations that form the graph + rgbdtools::KeyframeAssociationVector odometryEdges_; ///< odometry measurements between keyframes in the graph PathMsg path_msg_; /// < contains a vector of positions of the camera (not base) pose + + AffineTransform aggregatedPoseCorrection_; /** @brief processes an incoming RGBD frame with a given pose, * and determines whether a keyframe should be inserted @@ -377,6 +382,8 @@ class KeyframeMapper bool loadPath(const std::string& filepath); void updatePathFromKeyframePoses(); + + void publishAggregatedPoseCorrection(); }; } // namespace ccny_rgbd diff --git a/ccny_rgbd/launch/visual_odometry.launch b/ccny_rgbd/launch/visual_odometry.launch index f153c22..4af837f 100644 --- a/ccny_rgbd/launch/visual_odometry.launch +++ b/ccny_rgbd/launch/visual_odometry.launch @@ -59,7 +59,7 @@ #### registration ################################# - + #### registration: ICP Prob Model ################# diff --git a/ccny_rgbd/src/apps/keyframe_mapper.cpp b/ccny_rgbd/src/apps/keyframe_mapper.cpp index 59a3a46..e60c5a1 100644 --- a/ccny_rgbd/src/apps/keyframe_mapper.cpp +++ b/ccny_rgbd/src/apps/keyframe_mapper.cpp @@ -48,6 +48,7 @@ KeyframeMapper::KeyframeMapper( "keyframe_associations", queue_size_); path_pub_ = nh_.advertise( "mapper_path", queue_size_); + pose_correction_pub_ = nh_.advertise("pose_correction_tfs", queue_size_); // **** services @@ -117,14 +118,17 @@ void KeyframeMapper::initParams() max_stdev_ = 0.03; if (!nh_private_.getParam ("max_map_z", max_map_z_)) max_map_z_ = std::numeric_limits::infinity(); - + if (!nh_private_.getParam ("online_graph_opt", online_graph_opt_)) + online_graph_opt_ = false; + + // configure graph detection int graph_n_keypoints; int graph_n_candidates; int graph_k_nearest_neighbors; bool graph_matcher_use_desc_ratio_test = true; - + if (!nh_private_.getParam ("graph/n_keypoints", graph_n_keypoints)) graph_n_keypoints = 500; if (!nh_private_.getParam ("graph/n_candidates", graph_n_candidates)) @@ -140,6 +144,8 @@ void KeyframeMapper::initParams() graph_detector_.setSACReestimateTf(false); graph_detector_.setSACSaveResults(false); graph_detector_.setVerbose(verbose); + + aggregatedPoseCorrection_.setIdentity(); } void KeyframeMapper::RGBDCallback( @@ -147,6 +153,7 @@ void KeyframeMapper::RGBDCallback( const ImageMsg::ConstPtr& depth_msg, const CameraInfoMsg::ConstPtr& info_msg) { + //ROS_INFO("RGBDCallback called."); tf::StampedTransform transform; const ros::Time& time = rgb_msg->header.stamp; @@ -226,16 +233,73 @@ void KeyframeMapper::addKeyframe( { rgbdtools::RGBDKeyframe keyframe(frame); keyframe.pose = pose; - + int associations_prev = associations_.size(); + if (manual_add_) { ROS_INFO("Adding frame manually"); manual_add_ = false; keyframe.manually_added = true; } + + if(online_graph_opt_){ + //First extract and add keypoints/features to keyframe + ROS_INFO("Extracting keypoints..."); + graph_detector_.extractFeatures(keyframe); + } + + //Add keyframe to keyframevector keyframes_.push_back(keyframe); + + //Record the odometry measured between consecutive keyframes (only if more than one keyframe exists) + if(keyframes_.size()>1){ + rgbdtools::KeyframeAssociation odometryEdge; + + int from_idx = keyframes_.size() - 2; + int to_idx = keyframes_.size() - 1; + + const AffineTransform& from_pose = keyframes_[from_idx].pose; + const AffineTransform& to_pose = keyframes_[to_idx].pose; + AffineTransform tf = from_pose.inverse() * to_pose; + + odometryEdge.kf_idx_a = from_idx; + odometryEdge.kf_idx_b = to_idx; + odometryEdge.a2b = tf; + + odometryEdges_.push_back(odometryEdge); + } + + if(online_graph_opt_){ + graph_detector_.prepareMatcher(keyframes_); + //ROS_INFO("Keyframe associations being checked..."); + graph_detector_.onlineLoopClosureDetector(keyframes_, associations_); + ROS_INFO("Number of associations :%d", (int)associations_.size()); + if(associations_.size()>associations_prev){ + graph_solver_.solve(keyframes_, associations_,odometryEdges_); + updatePathFromKeyframePoses(); + + publishPath(); + publishKeyframePoses(); + publishKeyframeAssociations(); + + //Calculate and publish pose correction + AffineTransform poseCorrection; + poseCorrection = pose.inverse() * keyframes_[keyframes_.size()-1].pose; + aggregatedPoseCorrection_=poseCorrection * aggregatedPoseCorrection_; + publishAggregatedPoseCorrection(); + + } + } } +void KeyframeMapper::publishAggregatedPoseCorrection(){ + geometry_msgs::Transform msg; + tf::Transform correctiontf = tfFromEigenAffine(aggregatedPoseCorrection_); + tf::transformTFToMsg(correctiontf,msg); + pose_correction_pub_.publish(msg); +} + + bool KeyframeMapper::publishKeyframeSrvCallback( PublishKeyframe::Request& request, PublishKeyframe::Response& response) @@ -545,7 +609,7 @@ bool KeyframeMapper::solveGraphSrvCallback( ros::WallTime start = ros::WallTime::now(); // Graph solving: keyframe positions only, path is interpolated - graph_solver_.solve(keyframes_, associations_); + graph_solver_.solve(keyframes_, associations_,odometryEdges_); updatePathFromKeyframePoses(); // Graph solving: keyframe positions and VO path diff --git a/ccny_rgbd/src/ccny_rgbd/cfg/FeatureDetectorConfig.py b/ccny_rgbd/src/ccny_rgbd/cfg/FeatureDetectorConfig.py new file mode 100644 index 0000000..5ba33a9 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/cfg/FeatureDetectorConfig.py @@ -0,0 +1,74 @@ +## ********************************************************* +## +## File autogenerated for the ccny_rgbd package +## by the dynamic_reconfigure package. +## Please do not edit. +## +## ********************************************************/ + +##********************************************************** +## Software License Agreement (BSD License) +## +## Copyright (c) 2008, Willow Garage, Inc. +## All rights reserved. +## +## Redistribution and use in source and binary forms, with or without +## modification, are permitted provided that the following conditions +## are met: +## +## * Redistributions of source code must retain the above copyright +## notice, this list of conditions and the following disclaimer. +## * Redistributions in binary form must reproduce the above +## copyright notice, this list of conditions and the following +## disclaimer in the documentation and/or other materials provided +## with the distribution. +## * Neither the name of the Willow Garage nor the names of its +## contributors may be used to endorse or promote products derived +## from this software without specific prior written permission. +## +## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +## "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +## LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +## FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +## COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +## INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +## BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +## LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +## ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +## POSSIBILITY OF SUCH DAMAGE. +##**********************************************************/ + +from dynamic_reconfigure.encoding import extract_params + +inf = float('inf') + +config_description = {'upper': 'DEFAULT', 'lower': 'groups', 'srcline': 233, 'name': 'Default', 'parent': 0, 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'cstate': 'true', 'parentname': 'Default', 'class': 'DEFAULT', 'field': 'default', 'state': True, 'parentclass': '', 'groups': [], 'parameters': [{'srcline': 259, 'description': 'Detector type', 'max': '', 'cconsttype': 'const char * const', 'ctype': 'std::string', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'detector_type', 'edit_method': "{'enum_description': 'An enum to set detector type', 'enum': [{'srcline': 12, 'description': 'GoodFeaturesToTrack', 'srcfile': '../cfg/FeatureDetector.cfg', 'cconsttype': 'const char * const', 'value': 'GFT', 'ctype': 'std::string', 'type': 'str', 'name': 'GFT'}, {'srcline': 12, 'description': 'GoodFeaturesToTrack', 'srcfile': '../cfg/FeatureDetector.cfg', 'cconsttype': 'const char * const', 'value': 'STAR', 'ctype': 'std::string', 'type': 'str', 'name': 'STAR'}, {'srcline': 12, 'description': 'GoodFeaturesToTrack', 'srcfile': '../cfg/FeatureDetector.cfg', 'cconsttype': 'const char * const', 'value': 'ORB', 'ctype': 'std::string', 'type': 'str', 'name': 'ORB'}, {'srcline': 12, 'description': 'GoodFeaturesToTrack', 'srcfile': '../cfg/FeatureDetector.cfg', 'cconsttype': 'const char * const', 'value': 'SURF', 'ctype': 'std::string', 'type': 'str', 'name': 'SURF'}]}", 'default': 'GFT', 'level': 0, 'min': '', 'type': 'str'}, {'srcline': 259, 'description': 'Smoothing window half-size (pixels)', 'max': 5, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'smooth', 'edit_method': '', 'default': 0, 'level': 0, 'min': 0, 'type': 'int'}, {'srcline': 259, 'description': 'Maximum z-depth (meters)', 'max': 15.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'max_range', 'edit_method': '', 'default': 5.5, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'Maximum std_dev(z) (meters)', 'max': 0.2, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'max_stdev', 'edit_method': '', 'default': 0.03, 'level': 0, 'min': 0.0, 'type': 'double'}, {'srcline': 259, 'description': 'Show 2D keypoint image', 'max': True, 'cconsttype': 'const bool', 'ctype': 'bool', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'show_keypoints', 'edit_method': '', 'default': False, 'level': 0, 'min': False, 'type': 'bool'}, {'srcline': 259, 'description': 'Publish feature point cloud', 'max': True, 'cconsttype': 'const bool', 'ctype': 'bool', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'publish_cloud', 'edit_method': '', 'default': False, 'level': 0, 'min': False, 'type': 'bool'}, {'srcline': 259, 'description': 'Publish feature covariance markers', 'max': True, 'cconsttype': 'const bool', 'ctype': 'bool', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'publish_covariances', 'edit_method': '', 'default': False, 'level': 0, 'min': False, 'type': 'bool'}], 'type': '', 'id': 0} + +min = {} +max = {} +defaults = {} +level = {} +type = {} +all_level = 0 + +#def extract_params(config): +# params = [] +# params.extend(config['parameters']) +# for group in config['groups']: +# params.extend(extract_params(group)) +# return params + +for param in extract_params(config_description): + min[param['name']] = param['min'] + max[param['name']] = param['max'] + defaults[param['name']] = param['default'] + level[param['name']] = param['level'] + type[param['name']] = param['type'] + all_level = all_level | param['level'] + +FeatureDetector_GFT = 'GFT' +FeatureDetector_STAR = 'STAR' +FeatureDetector_ORB = 'ORB' +FeatureDetector_SURF = 'SURF' diff --git a/ccny_rgbd/src/ccny_rgbd/cfg/GftDetectorConfig.py b/ccny_rgbd/src/ccny_rgbd/cfg/GftDetectorConfig.py new file mode 100644 index 0000000..47b5ee5 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/cfg/GftDetectorConfig.py @@ -0,0 +1,70 @@ +## ********************************************************* +## +## File autogenerated for the ccny_rgbd package +## by the dynamic_reconfigure package. +## Please do not edit. +## +## ********************************************************/ + +##********************************************************** +## Software License Agreement (BSD License) +## +## Copyright (c) 2008, Willow Garage, Inc. +## All rights reserved. +## +## Redistribution and use in source and binary forms, with or without +## modification, are permitted provided that the following conditions +## are met: +## +## * Redistributions of source code must retain the above copyright +## notice, this list of conditions and the following disclaimer. +## * Redistributions in binary form must reproduce the above +## copyright notice, this list of conditions and the following +## disclaimer in the documentation and/or other materials provided +## with the distribution. +## * Neither the name of the Willow Garage nor the names of its +## contributors may be used to endorse or promote products derived +## from this software without specific prior written permission. +## +## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +## "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +## LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +## FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +## COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +## INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +## BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +## LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +## ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +## POSSIBILITY OF SUCH DAMAGE. +##**********************************************************/ + +from dynamic_reconfigure.encoding import extract_params + +inf = float('inf') + +config_description = {'upper': 'DEFAULT', 'lower': 'groups', 'srcline': 233, 'name': 'Default', 'parent': 0, 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'cstate': 'true', 'parentname': 'Default', 'class': 'DEFAULT', 'field': 'default', 'state': True, 'parentclass': '', 'groups': [], 'parameters': [{'srcline': 259, 'description': 'Number of feautures requested', 'max': 1000, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'n_features', 'edit_method': '', 'default': 400, 'level': 0, 'min': 1, 'type': 'int'}, {'srcline': 259, 'description': 'Minimum distance between features (pixels)', 'max': 15, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'min_distance', 'edit_method': '', 'default': 1, 'level': 0, 'min': 0, 'type': 'int'}], 'type': '', 'id': 0} + +min = {} +max = {} +defaults = {} +level = {} +type = {} +all_level = 0 + +#def extract_params(config): +# params = [] +# params.extend(config['parameters']) +# for group in config['groups']: +# params.extend(extract_params(group)) +# return params + +for param in extract_params(config_description): + min[param['name']] = param['min'] + max[param['name']] = param['max'] + defaults[param['name']] = param['default'] + level[param['name']] = param['level'] + type[param['name']] = param['type'] + all_level = all_level | param['level'] + diff --git a/ccny_rgbd/src/ccny_rgbd/cfg/OrbDetectorConfig.py b/ccny_rgbd/src/ccny_rgbd/cfg/OrbDetectorConfig.py new file mode 100644 index 0000000..e41f82c --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/cfg/OrbDetectorConfig.py @@ -0,0 +1,70 @@ +## ********************************************************* +## +## File autogenerated for the ccny_rgbd package +## by the dynamic_reconfigure package. +## Please do not edit. +## +## ********************************************************/ + +##********************************************************** +## Software License Agreement (BSD License) +## +## Copyright (c) 2008, Willow Garage, Inc. +## All rights reserved. +## +## Redistribution and use in source and binary forms, with or without +## modification, are permitted provided that the following conditions +## are met: +## +## * Redistributions of source code must retain the above copyright +## notice, this list of conditions and the following disclaimer. +## * Redistributions in binary form must reproduce the above +## copyright notice, this list of conditions and the following +## disclaimer in the documentation and/or other materials provided +## with the distribution. +## * Neither the name of the Willow Garage nor the names of its +## contributors may be used to endorse or promote products derived +## from this software without specific prior written permission. +## +## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +## "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +## LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +## FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +## COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +## INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +## BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +## LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +## ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +## POSSIBILITY OF SUCH DAMAGE. +##**********************************************************/ + +from dynamic_reconfigure.encoding import extract_params + +inf = float('inf') + +config_description = {'upper': 'DEFAULT', 'lower': 'groups', 'srcline': 233, 'name': 'Default', 'parent': 0, 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'cstate': 'true', 'parentname': 'Default', 'class': 'DEFAULT', 'field': 'default', 'state': True, 'parentclass': '', 'groups': [], 'parameters': [{'srcline': 259, 'description': 'Number of feautures requested', 'max': 1000, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'n_features', 'edit_method': '', 'default': 400, 'level': 0, 'min': 1, 'type': 'int'}, {'srcline': 259, 'description': 'Detection threshold', 'max': 200.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'threshold', 'edit_method': '', 'default': 31.0, 'level': 0, 'min': 1.0, 'type': 'double'}], 'type': '', 'id': 0} + +min = {} +max = {} +defaults = {} +level = {} +type = {} +all_level = 0 + +#def extract_params(config): +# params = [] +# params.extend(config['parameters']) +# for group in config['groups']: +# params.extend(extract_params(group)) +# return params + +for param in extract_params(config_description): + min[param['name']] = param['min'] + max[param['name']] = param['max'] + defaults[param['name']] = param['default'] + level[param['name']] = param['level'] + type[param['name']] = param['type'] + all_level = all_level | param['level'] + diff --git a/ccny_rgbd/src/ccny_rgbd/cfg/RGBDImageProcConfig.py b/ccny_rgbd/src/ccny_rgbd/cfg/RGBDImageProcConfig.py new file mode 100644 index 0000000..189d877 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/cfg/RGBDImageProcConfig.py @@ -0,0 +1,70 @@ +## ********************************************************* +## +## File autogenerated for the ccny_rgbd package +## by the dynamic_reconfigure package. +## Please do not edit. +## +## ********************************************************/ + +##********************************************************** +## Software License Agreement (BSD License) +## +## Copyright (c) 2008, Willow Garage, Inc. +## All rights reserved. +## +## Redistribution and use in source and binary forms, with or without +## modification, are permitted provided that the following conditions +## are met: +## +## * Redistributions of source code must retain the above copyright +## notice, this list of conditions and the following disclaimer. +## * Redistributions in binary form must reproduce the above +## copyright notice, this list of conditions and the following +## disclaimer in the documentation and/or other materials provided +## with the distribution. +## * Neither the name of the Willow Garage nor the names of its +## contributors may be used to endorse or promote products derived +## from this software without specific prior written permission. +## +## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +## "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +## LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +## FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +## COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +## INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +## BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +## LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +## ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +## POSSIBILITY OF SUCH DAMAGE. +##**********************************************************/ + +from dynamic_reconfigure.encoding import extract_params + +inf = float('inf') + +config_description = {'upper': 'DEFAULT', 'lower': 'groups', 'srcline': 233, 'name': 'Default', 'parent': 0, 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'cstate': 'true', 'parentname': 'Default', 'class': 'DEFAULT', 'field': 'default', 'state': True, 'parentclass': '', 'groups': [], 'parameters': [{'srcline': 259, 'description': 'Resampling scale', 'max': 1.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'scale', 'edit_method': '', 'default': 0.5, 'level': 0, 'min': 0.05, 'type': 'double'}, {'srcline': 259, 'description': 'Publish point cloud', 'max': True, 'cconsttype': 'const bool', 'ctype': 'bool', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'publish_cloud', 'edit_method': '', 'default': True, 'level': 0, 'min': False, 'type': 'bool'}], 'type': '', 'id': 0} + +min = {} +max = {} +defaults = {} +level = {} +type = {} +all_level = 0 + +#def extract_params(config): +# params = [] +# params.extend(config['parameters']) +# for group in config['groups']: +# params.extend(extract_params(group)) +# return params + +for param in extract_params(config_description): + min[param['name']] = param['min'] + max[param['name']] = param['max'] + defaults[param['name']] = param['default'] + level[param['name']] = param['level'] + type[param['name']] = param['type'] + all_level = all_level | param['level'] + diff --git a/ccny_rgbd/src/ccny_rgbd/cfg/StarDetectorConfig.py b/ccny_rgbd/src/ccny_rgbd/cfg/StarDetectorConfig.py new file mode 100644 index 0000000..5ce0a48 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/cfg/StarDetectorConfig.py @@ -0,0 +1,70 @@ +## ********************************************************* +## +## File autogenerated for the ccny_rgbd package +## by the dynamic_reconfigure package. +## Please do not edit. +## +## ********************************************************/ + +##********************************************************** +## Software License Agreement (BSD License) +## +## Copyright (c) 2008, Willow Garage, Inc. +## All rights reserved. +## +## Redistribution and use in source and binary forms, with or without +## modification, are permitted provided that the following conditions +## are met: +## +## * Redistributions of source code must retain the above copyright +## notice, this list of conditions and the following disclaimer. +## * Redistributions in binary form must reproduce the above +## copyright notice, this list of conditions and the following +## disclaimer in the documentation and/or other materials provided +## with the distribution. +## * Neither the name of the Willow Garage nor the names of its +## contributors may be used to endorse or promote products derived +## from this software without specific prior written permission. +## +## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +## "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +## LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +## FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +## COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +## INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +## BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +## LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +## ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +## POSSIBILITY OF SUCH DAMAGE. +##**********************************************************/ + +from dynamic_reconfigure.encoding import extract_params + +inf = float('inf') + +config_description = {'upper': 'DEFAULT', 'lower': 'groups', 'srcline': 233, 'name': 'Default', 'parent': 0, 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'cstate': 'true', 'parentname': 'Default', 'class': 'DEFAULT', 'field': 'default', 'state': True, 'parentclass': '', 'groups': [], 'parameters': [{'srcline': 259, 'description': 'Detection threshold', 'max': 100.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'threshold', 'edit_method': '', 'default': 32.0, 'level': 0, 'min': 1.0, 'type': 'double'}, {'srcline': 259, 'description': 'Minimum distance between features (pixels)', 'max': 15, 'cconsttype': 'const int', 'ctype': 'int', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'min_distance', 'edit_method': '', 'default': 2, 'level': 0, 'min': 0, 'type': 'int'}], 'type': '', 'id': 0} + +min = {} +max = {} +defaults = {} +level = {} +type = {} +all_level = 0 + +#def extract_params(config): +# params = [] +# params.extend(config['parameters']) +# for group in config['groups']: +# params.extend(extract_params(group)) +# return params + +for param in extract_params(config_description): + min[param['name']] = param['min'] + max[param['name']] = param['max'] + defaults[param['name']] = param['default'] + level[param['name']] = param['level'] + type[param['name']] = param['type'] + all_level = all_level | param['level'] + diff --git a/ccny_rgbd/src/ccny_rgbd/cfg/SurfDetectorConfig.py b/ccny_rgbd/src/ccny_rgbd/cfg/SurfDetectorConfig.py new file mode 100644 index 0000000..caeae27 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/cfg/SurfDetectorConfig.py @@ -0,0 +1,70 @@ +## ********************************************************* +## +## File autogenerated for the ccny_rgbd package +## by the dynamic_reconfigure package. +## Please do not edit. +## +## ********************************************************/ + +##********************************************************** +## Software License Agreement (BSD License) +## +## Copyright (c) 2008, Willow Garage, Inc. +## All rights reserved. +## +## Redistribution and use in source and binary forms, with or without +## modification, are permitted provided that the following conditions +## are met: +## +## * Redistributions of source code must retain the above copyright +## notice, this list of conditions and the following disclaimer. +## * Redistributions in binary form must reproduce the above +## copyright notice, this list of conditions and the following +## disclaimer in the documentation and/or other materials provided +## with the distribution. +## * Neither the name of the Willow Garage nor the names of its +## contributors may be used to endorse or promote products derived +## from this software without specific prior written permission. +## +## THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +## "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +## LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +## FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +## COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +## INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +## BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +## LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +## CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +## LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +## ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +## POSSIBILITY OF SUCH DAMAGE. +##**********************************************************/ + +from dynamic_reconfigure.encoding import extract_params + +inf = float('inf') + +config_description = {'upper': 'DEFAULT', 'lower': 'groups', 'srcline': 233, 'name': 'Default', 'parent': 0, 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'cstate': 'true', 'parentname': 'Default', 'class': 'DEFAULT', 'field': 'default', 'state': True, 'parentclass': '', 'groups': [], 'parameters': [{'srcline': 259, 'description': 'Detection threshold', 'max': 2000.0, 'cconsttype': 'const double', 'ctype': 'double', 'srcfile': '/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py', 'name': 'threshold', 'edit_method': '', 'default': 400.0, 'level': 0, 'min': 1.0, 'type': 'double'}], 'type': '', 'id': 0} + +min = {} +max = {} +defaults = {} +level = {} +type = {} +all_level = 0 + +#def extract_params(config): +# params = [] +# params.extend(config['parameters']) +# for group in config['groups']: +# params.extend(extract_params(group)) +# return params + +for param in extract_params(config_description): + min[param['name']] = param['min'] + max[param['name']] = param['max'] + defaults[param['name']] = param['default'] + level[param['name']] = param['level'] + type[param['name']] = param['type'] + all_level = all_level | param['level'] + diff --git a/ccny_rgbd/src/ccny_rgbd/srv/_AddManualKeyframe.py b/ccny_rgbd/src/ccny_rgbd/srv/_AddManualKeyframe.py new file mode 100644 index 0000000..fe084f9 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/srv/_AddManualKeyframe.py @@ -0,0 +1,176 @@ +"""autogenerated by genpy from ccny_rgbd/AddManualKeyframeRequest.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class AddManualKeyframeRequest(genpy.Message): + _md5sum = "d41d8cd98f00b204e9800998ecf8427e" + _type = "ccny_rgbd/AddManualKeyframeRequest" + _has_header = False #flag to mark the presence of a Header object + _full_text = """ +""" + __slots__ = [] + _slot_types = [] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(AddManualKeyframeRequest, self).__init__(*args, **kwds) + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +"""autogenerated by genpy from ccny_rgbd/AddManualKeyframeResponse.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class AddManualKeyframeResponse(genpy.Message): + _md5sum = "d41d8cd98f00b204e9800998ecf8427e" + _type = "ccny_rgbd/AddManualKeyframeResponse" + _has_header = False #flag to mark the presence of a Header object + _full_text = """ + +""" + __slots__ = [] + _slot_types = [] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(AddManualKeyframeResponse, self).__init__(*args, **kwds) + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +class AddManualKeyframe(object): + _type = 'ccny_rgbd/AddManualKeyframe' + _md5sum = 'd41d8cd98f00b204e9800998ecf8427e' + _request_class = AddManualKeyframeRequest + _response_class = AddManualKeyframeResponse diff --git a/ccny_rgbd/src/ccny_rgbd/srv/_GenerateGraph.py b/ccny_rgbd/src/ccny_rgbd/srv/_GenerateGraph.py new file mode 100644 index 0000000..83b7e30 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/srv/_GenerateGraph.py @@ -0,0 +1,176 @@ +"""autogenerated by genpy from ccny_rgbd/GenerateGraphRequest.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class GenerateGraphRequest(genpy.Message): + _md5sum = "d41d8cd98f00b204e9800998ecf8427e" + _type = "ccny_rgbd/GenerateGraphRequest" + _has_header = False #flag to mark the presence of a Header object + _full_text = """ +""" + __slots__ = [] + _slot_types = [] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(GenerateGraphRequest, self).__init__(*args, **kwds) + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +"""autogenerated by genpy from ccny_rgbd/GenerateGraphResponse.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class GenerateGraphResponse(genpy.Message): + _md5sum = "d41d8cd98f00b204e9800998ecf8427e" + _type = "ccny_rgbd/GenerateGraphResponse" + _has_header = False #flag to mark the presence of a Header object + _full_text = """ + +""" + __slots__ = [] + _slot_types = [] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(GenerateGraphResponse, self).__init__(*args, **kwds) + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +class GenerateGraph(object): + _type = 'ccny_rgbd/GenerateGraph' + _md5sum = 'd41d8cd98f00b204e9800998ecf8427e' + _request_class = GenerateGraphRequest + _response_class = GenerateGraphResponse diff --git a/ccny_rgbd/src/ccny_rgbd/srv/_Load.py b/ccny_rgbd/src/ccny_rgbd/srv/_Load.py new file mode 100644 index 0000000..0ddeec0 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/srv/_Load.py @@ -0,0 +1,210 @@ +"""autogenerated by genpy from ccny_rgbd/LoadRequest.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class LoadRequest(genpy.Message): + _md5sum = "030824f52a0628ead956fb9d67e66ae9" + _type = "ccny_rgbd/LoadRequest" + _has_header = False #flag to mark the presence of a Header object + _full_text = """string filename + +""" + __slots__ = ['filename'] + _slot_types = ['string'] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + filename + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(LoadRequest, self).__init__(*args, **kwds) + #message fields cannot be None, assign default values for those that are + if self.filename is None: + self.filename = '' + else: + self.filename = '' + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + _x = self.filename + length = len(_x) + if python3 or type(_x) == unicode: + _x = _x.encode('utf-8') + length = len(_x) + buff.write(struct.pack(' 0x03000000 else False +import genpy +import struct + + +class LoadResponse(genpy.Message): + _md5sum = "d41d8cd98f00b204e9800998ecf8427e" + _type = "ccny_rgbd/LoadResponse" + _has_header = False #flag to mark the presence of a Header object + _full_text = """ + +""" + __slots__ = [] + _slot_types = [] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(LoadResponse, self).__init__(*args, **kwds) + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +class Load(object): + _type = 'ccny_rgbd/Load' + _md5sum = '030824f52a0628ead956fb9d67e66ae9' + _request_class = LoadRequest + _response_class = LoadResponse diff --git a/ccny_rgbd/src/ccny_rgbd/srv/_PublishKeyframe.py b/ccny_rgbd/src/ccny_rgbd/srv/_PublishKeyframe.py new file mode 100644 index 0000000..2bc2385 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/srv/_PublishKeyframe.py @@ -0,0 +1,189 @@ +"""autogenerated by genpy from ccny_rgbd/PublishKeyframeRequest.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class PublishKeyframeRequest(genpy.Message): + _md5sum = "c5e4a7d59c68f74eabcec876a00216aa" + _type = "ccny_rgbd/PublishKeyframeRequest" + _has_header = False #flag to mark the presence of a Header object + _full_text = """int32 id + +""" + __slots__ = ['id'] + _slot_types = ['int32'] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + id + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(PublishKeyframeRequest, self).__init__(*args, **kwds) + #message fields cannot be None, assign default values for those that are + if self.id is None: + self.id = 0 + else: + self.id = 0 + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + buff.write(_struct_i.pack(self.id)) + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + start = end + end += 4 + (self.id,) = _struct_i.unpack(str[start:end]) + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + buff.write(_struct_i.pack(self.id)) + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + start = end + end += 4 + (self.id,) = _struct_i.unpack(str[start:end]) + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +_struct_i = struct.Struct(" 0x03000000 else False +import genpy +import struct + + +class PublishKeyframeResponse(genpy.Message): + _md5sum = "d41d8cd98f00b204e9800998ecf8427e" + _type = "ccny_rgbd/PublishKeyframeResponse" + _has_header = False #flag to mark the presence of a Header object + _full_text = """ + +""" + __slots__ = [] + _slot_types = [] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(PublishKeyframeResponse, self).__init__(*args, **kwds) + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +class PublishKeyframe(object): + _type = 'ccny_rgbd/PublishKeyframe' + _md5sum = 'c5e4a7d59c68f74eabcec876a00216aa' + _request_class = PublishKeyframeRequest + _response_class = PublishKeyframeResponse diff --git a/ccny_rgbd/src/ccny_rgbd/srv/_PublishKeyframes.py b/ccny_rgbd/src/ccny_rgbd/srv/_PublishKeyframes.py new file mode 100644 index 0000000..7f2f75c --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/srv/_PublishKeyframes.py @@ -0,0 +1,210 @@ +"""autogenerated by genpy from ccny_rgbd/PublishKeyframesRequest.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class PublishKeyframesRequest(genpy.Message): + _md5sum = "a91fd359d10bd35fdc46f24ffc722fa2" + _type = "ccny_rgbd/PublishKeyframesRequest" + _has_header = False #flag to mark the presence of a Header object + _full_text = """string re + +""" + __slots__ = ['re'] + _slot_types = ['string'] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + re + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(PublishKeyframesRequest, self).__init__(*args, **kwds) + #message fields cannot be None, assign default values for those that are + if self.re is None: + self.re = '' + else: + self.re = '' + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + _x = self.re + length = len(_x) + if python3 or type(_x) == unicode: + _x = _x.encode('utf-8') + length = len(_x) + buff.write(struct.pack(' 0x03000000 else False +import genpy +import struct + + +class PublishKeyframesResponse(genpy.Message): + _md5sum = "d41d8cd98f00b204e9800998ecf8427e" + _type = "ccny_rgbd/PublishKeyframesResponse" + _has_header = False #flag to mark the presence of a Header object + _full_text = """ + +""" + __slots__ = [] + _slot_types = [] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(PublishKeyframesResponse, self).__init__(*args, **kwds) + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +class PublishKeyframes(object): + _type = 'ccny_rgbd/PublishKeyframes' + _md5sum = 'a91fd359d10bd35fdc46f24ffc722fa2' + _request_class = PublishKeyframesRequest + _response_class = PublishKeyframesResponse diff --git a/ccny_rgbd/src/ccny_rgbd/srv/_Save.py b/ccny_rgbd/src/ccny_rgbd/srv/_Save.py new file mode 100644 index 0000000..faea5a6 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/srv/_Save.py @@ -0,0 +1,210 @@ +"""autogenerated by genpy from ccny_rgbd/SaveRequest.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class SaveRequest(genpy.Message): + _md5sum = "030824f52a0628ead956fb9d67e66ae9" + _type = "ccny_rgbd/SaveRequest" + _has_header = False #flag to mark the presence of a Header object + _full_text = """string filename + +""" + __slots__ = ['filename'] + _slot_types = ['string'] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + filename + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(SaveRequest, self).__init__(*args, **kwds) + #message fields cannot be None, assign default values for those that are + if self.filename is None: + self.filename = '' + else: + self.filename = '' + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + _x = self.filename + length = len(_x) + if python3 or type(_x) == unicode: + _x = _x.encode('utf-8') + length = len(_x) + buff.write(struct.pack(' 0x03000000 else False +import genpy +import struct + + +class SaveResponse(genpy.Message): + _md5sum = "d41d8cd98f00b204e9800998ecf8427e" + _type = "ccny_rgbd/SaveResponse" + _has_header = False #flag to mark the presence of a Header object + _full_text = """ + +""" + __slots__ = [] + _slot_types = [] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(SaveResponse, self).__init__(*args, **kwds) + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +class Save(object): + _type = 'ccny_rgbd/Save' + _md5sum = '030824f52a0628ead956fb9d67e66ae9' + _request_class = SaveRequest + _response_class = SaveResponse diff --git a/ccny_rgbd/src/ccny_rgbd/srv/_SolveGraph.py b/ccny_rgbd/src/ccny_rgbd/srv/_SolveGraph.py new file mode 100644 index 0000000..ebb5d50 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/srv/_SolveGraph.py @@ -0,0 +1,176 @@ +"""autogenerated by genpy from ccny_rgbd/SolveGraphRequest.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class SolveGraphRequest(genpy.Message): + _md5sum = "d41d8cd98f00b204e9800998ecf8427e" + _type = "ccny_rgbd/SolveGraphRequest" + _has_header = False #flag to mark the presence of a Header object + _full_text = """ +""" + __slots__ = [] + _slot_types = [] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(SolveGraphRequest, self).__init__(*args, **kwds) + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +"""autogenerated by genpy from ccny_rgbd/SolveGraphResponse.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + + +class SolveGraphResponse(genpy.Message): + _md5sum = "d41d8cd98f00b204e9800998ecf8427e" + _type = "ccny_rgbd/SolveGraphResponse" + _has_header = False #flag to mark the presence of a Header object + _full_text = """ + +""" + __slots__ = [] + _slot_types = [] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(SolveGraphResponse, self).__init__(*args, **kwds) + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize(self, str): + """ + unpack serialized message in str into this message instance + :param str: byte array of serialized message, ``str`` + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + + + def serialize_numpy(self, buff, numpy): + """ + serialize message with numpy array types into buffer + :param buff: buffer, ``StringIO`` + :param numpy: numpy python module + """ + try: + pass + except struct.error as se: self._check_types(se) + except TypeError as te: self._check_types(te) + + def deserialize_numpy(self, str, numpy): + """ + unpack serialized message in str into this message instance using numpy for array types + :param str: byte array of serialized message, ``str`` + :param numpy: numpy python module + """ + try: + end = 0 + return self + except struct.error as e: + raise genpy.DeserializationError(e) #most likely buffer underfill + +_struct_I = genpy.struct_I +class SolveGraph(object): + _type = 'ccny_rgbd/SolveGraph' + _md5sum = 'd41d8cd98f00b204e9800998ecf8427e' + _request_class = SolveGraphRequest + _response_class = SolveGraphResponse diff --git a/lib_rgbdtools/include/rgbdtools/features/feature_detector.h b/lib_rgbdtools/include/rgbdtools/features/feature_detector.h new file mode 100644 index 0000000..b6a9f81 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/features/feature_detector.h @@ -0,0 +1,121 @@ +/** + * @file feature_detector.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_FEATURE_DETECTOR_H +#define RGBDTOOLS_FEATURE_DETECTOR_H + +#include +#include + +#include "rgbdtools/types.h" +#include "rgbdtools/rgbd_frame.h" + +namespace rgbdtools { + +/** @brief Base class for sparse feature extractors + */ +class FeatureDetector +{ + public: + + /** @brief Default constructor + */ + FeatureDetector(); + + /** @brief Default destructor + */ + virtual ~FeatureDetector(); + + /** @brief Main function to call to detect the sparse features + * in an RGBDFrame and fill out the corresponding information + * @param frame the input frame + */ + void findFeatures(RGBDFrame& frame); + + /** @brief Returns the smoothing size. + * + * Smoothing is performed using Gaussian bluring in a window of size + * smooth*2 + 1 + * + * If smooth is set to 0, then no blurring will take place + * + * @return smoothing window size + */ + inline int getSmooth() const; + + /** @brief Returns the maximum allowed z-depth (in meters) for features + * @return maximum allowed z-depth (in meters) for features + */ + inline double getMaxRange() const; + + /** @brief Returns the maximum allowed std_dev(z) (in meters) for features + * @return maximum allowed std_dev(z) (in meters) for features + */ + inline double getMaxStDev() const; + + /** @brief Sets the smoothing size. + * + * Smoothing is performed using Gaussian bluring in a window of size + * smooth*2 + 1 + * + * If smooth is set to 0, then no blurring will take place + * + * @param smooth smoothing window size + */ + void setSmooth(int smooth); + + /** @brief Sets the maximum allowed z-depth (in meters) for features + * @param max_range maximum allowed z-depth (in meters) for features + */ + void setMaxRange(double max_range); + + /** @brief Sets the maximum allowed std_dev(z) (in meters) for features + * @param max_stdev maximum allowed std_dev(z) (in meters) for features + */ + void setMaxStDev(double max_stdev); + + protected: + + boost::mutex mutex_; ///< state mutex + + bool compute_descriptors_; ///< whether to calculate feature descriptors + + /** @brief Implementation of the feature detector. + * @param frame the input frame + * @param input_img the image for feature detection, derived from the + * RGB image of the frame after (optional) blurring + */ + virtual void findFeatures(RGBDFrame& frame, const cv::Mat& input_img) = 0; + + private: + + int smooth_; ///< blurring size (blur winddow = smooth*2 + 1) + double max_range_; ///< maximum allowed z-depth (in meters) for features + double max_stdev_; ///< maximum allowed std_dev(z) (in meters) for features +}; + +typedef boost::shared_ptr FeatureDetectorPtr; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_FEATURE_DETECTOR_H diff --git a/lib_rgbdtools/include/rgbdtools/features/gft_detector.h b/lib_rgbdtools/include/rgbdtools/features/gft_detector.h new file mode 100644 index 0000000..deecd48 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/features/gft_detector.h @@ -0,0 +1,77 @@ +/** + * @file gft_detector.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_GFT_DETECTOR_H +#define RGBDTOOLS_GFT_DETECTOR_H + +#include +#include + +#include "rgbdtools/features/feature_detector.h" + +namespace rgbdtools { + +/** @brief GoodFeaturesToTrack detector + */ +class GftDetector: public FeatureDetector +{ + public: + + /** @brief Default constructor + */ + GftDetector(); + + /** @brief Default destructor + */ + ~GftDetector(); + + /** @brief Implementation of the feature detector. + * @param frame the input frame + * @param input_img the image for feature detection, derived from the + * RGB image of the frame after (optional) blurring + */ + void findFeatures(RGBDFrame& frame, const cv::Mat& input_img); + + /** @brief Set the desired number of features + * @param n_features desired number of features + */ + void setNFeatures(int n_features); + + /** @brief Set the minimum distance (in pixels) between the features + * @param min_distance minimum distance (in pixels) between the features + */ + void setMinDistance(double min_distance); + + private: + + int n_features_; ///< the number of desired features + double min_distance_; ///< the minimum distance (in pixels) between the features + + boost::shared_ptr gft_detector_; ///< OpenCV GTF detector object +}; + +typedef boost::shared_ptr GftDetectorPtr; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_GFT_DETECTOR_H diff --git a/lib_rgbdtools/include/rgbdtools/features/orb_detector.h b/lib_rgbdtools/include/rgbdtools/features/orb_detector.h new file mode 100644 index 0000000..cc981ed --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/features/orb_detector.h @@ -0,0 +1,78 @@ +/** + * @file orb_detector.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_ORB_DETECTOR_H +#define RGBDTOOLS_ORB_DETECTOR_H + +#include +#include + +#include "rgbdtools/features/feature_detector.h" + +namespace rgbdtools { + +/** @brief ORB detector +*/ +class OrbDetector: public FeatureDetector +{ + public: + + /** @brief Default constructor + */ + OrbDetector(); + + /** @brief Default destructor + */ + ~OrbDetector(); + + /** @brief Implementation of the feature detector. + * @param frame the input frame + * @param input_img the image for feature detection, derived from the + * RGB image of the frame after (optional) blurring + */ + void findFeatures(RGBDFrame& frame, const cv::Mat& input_img); + + /** @brief Sets the detection threshold + * @param threshold the detection threshold + */ + void setThreshold(int threshold); + + /** @brief Sets the number of desired features + * @param n_features number of desired features + */ + void setNFeatures(int n_features); + + private: + + int n_features_; ///< number of desired features + double threshold_; ///< threshold for detection + + cv::OrbDescriptorExtractor orb_descriptor_; ///< OpenCV feature detector object + boost::shared_ptr orb_detector_; ///< OpenCV descriptor extractor object +}; + +typedef boost::shared_ptr OrbDetectorPtr; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_ORB_DETECTOR_H diff --git a/lib_rgbdtools/include/rgbdtools/features/star_detector.h b/lib_rgbdtools/include/rgbdtools/features/star_detector.h new file mode 100644 index 0000000..8b0eea0 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/features/star_detector.h @@ -0,0 +1,77 @@ +/** + * @file star_detector.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_STAR_DETECTOR_H +#define RGBDTOOLS_STAR_DETECTOR_H + +#include +#include + +#include "rgbdtools/features/feature_detector.h" + +namespace rgbdtools { + +/** @brief STAR detector +*/ +class StarDetector: public FeatureDetector +{ + public: + + /** @brief Default constructor + */ + StarDetector(); + + /** @brief Default destructor + */ + ~StarDetector(); + + /** @brief Implementation of the feature detector. + * @param frame the input frame + * @param input_img the image for feature detection, derived from the + * RGB image of the frame after (optional) blurring + */ + void findFeatures(RGBDFrame& frame, const cv::Mat& input_img); + + /** @brief Set the minimum distance (in pixels) between the features + * @param min_distance minimum distance (in pixels) between the features + */ + void setMinDistance(double min_distance); + + /** @brief Set the threshold for detection + * @param threshold threshold for detection + */ + void setThreshold(double threshold); + + private: + + double min_distance_; ///< the minimum distance (in pixels) between the features + double threshold_; ///< threshold for detection + + boost::shared_ptr star_detector_; ///< OpenCV detector class +}; + +typedef boost::shared_ptr StarDetectorPtr; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_STAR_DETECTOR_H diff --git a/lib_rgbdtools/include/rgbdtools/features/surf_detector.h b/lib_rgbdtools/include/rgbdtools/features/surf_detector.h new file mode 100644 index 0000000..446e8b3 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/features/surf_detector.h @@ -0,0 +1,74 @@ +/** + * @file surf_detector.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_SURF_DETECTOR_H +#define RGBDTOOLS_SURF_DETECTOR_H + +#include +#include + +#include "rgbdtools/features/feature_detector.h" + +namespace rgbdtools { + +/** @brief SURF detector +*/ +class SurfDetector: public FeatureDetector +{ + public: + + /** @brief Default constructor + */ + SurfDetector(); + + /** @brief Default destructor + */ + ~SurfDetector(); + + /** @brief Implementation of the feature detector. + * @param frame the input frame + * @param input_img the image for feature detection, derived from the + * RGB image of the frame after (optional) blurring + */ + void findFeatures(RGBDFrame& frame, const cv::Mat& input_img); + + /** @brief Set the threshold for detection + * @param threshold threshold for detection + */ + void setThreshold(double threshold); + + private: + + double threshold_; ///< threshold for detection + + /** @brief OpenCV feature detector object */ + boost::shared_ptr surf_detector_; + + cv::SurfDescriptorExtractor surf_descriptor_; ///< OpenCV descriptor extractor object +}; + +typedef boost::shared_ptr SurfDetectorPtr; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_SURF_DETECTOR_H diff --git a/lib_rgbdtools/include/rgbdtools/graph/keyframe_association.h b/lib_rgbdtools/include/rgbdtools/graph/keyframe_association.h new file mode 100644 index 0000000..14bd19d --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/graph/keyframe_association.h @@ -0,0 +1,77 @@ +/** + * @file keyframe_association.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_KEYFRAME_ASSOCIATION_H +#define RGBDTOOLS_KEYFRAME_ASSOCIATION_H + +#include +#include + +#include "rgbdtools/types.h" + +namespace rgbdtools { + +/** @brief Class representing an association between two keyframes, + * used for graph-based pose alignement. + * + * Association types: + * - VO: from visual odometry + * - RANSAC: from RANSAC-based sparse feature matching + * - ODOMETRY: from classical odometry sources + */ + +class KeyframeAssociation +{ + public: + + /** @brief Association types + * + * - VO: from visual odometry + * - RANSAC: from RANSAC-based sparse feature matching + * - ODOMETRY: from classical odometry sources + */ + enum Type {VO, RANSAC, ODOMETRY}; + + int kf_idx_a; ///< index of keyframe A + int kf_idx_b; ///< index of keyframe B + + Type type; ///< source of the association + + std::vector matches; ///< for type=RANSAC, vector of RANSAC inliers mtaches + + /** @brief Transform between the two keyframe poses + * + * The transform is expressed in A's coordinate frame. + * A and B's poses are expressed in the fixed frame. + * + * B.pose = A.pose * a2b + */ + AffineTransform a2b; +}; + +typedef Eigen::aligned_allocator KeyframeAssociationAllocator; +typedef std::vector KeyframeAssociationVector; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_KEYFRAME_ASSOCIATION_H diff --git a/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_detector.h b/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_detector.h new file mode 100644 index 0000000..659becc --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_detector.h @@ -0,0 +1,251 @@ +/** + * @file keyframe_graph_detector.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_KEYFRAME_GRAPH_DETECTOR_H +#define RGBDTOOLS_KEYFRAME_GRAPH_DETECTOR_H + +#include +#include +#include +#include + +#include "rgbdtools/types.h" +#include "rgbdtools/rgbd_keyframe.h" +#include "rgbdtools/map_util.h" +#include "rgbdtools/graph/keyframe_association.h" + +namespace rgbdtools { + +/** @brief Detects graph correspondences based on visual feature + * matching between keyframes. + */ +class KeyframeGraphDetector +{ + public: + + enum CandidateGenerationMethod + { + CANDIDATE_GENERATION_BRUTE_FORCE, + CANDIDATE_GENERATION_SURF_TREE + }; + + enum PairwiseMatchingMethod + { + PAIRWISE_MATCHING_BFSAC, + PAIRWISE_MATCHING_RANSAC, + }; + + enum PairwiseMatcherIndex + { + PAIRWISE_MATCHER_LINEAR, + PAIRWISE_MATCHER_KDTREE, + }; + + /** @brief Constructor from ROS nodehandles + * @param nh the public nodehandle + * @param nh_private the private nodehandle + */ + KeyframeGraphDetector(); + + /** @brief Default destructor + */ + virtual ~KeyframeGraphDetector(); + + void setVerbose(bool verbose); + void setNCandidates(int n_candidates); + void setKNearestNeighbors(int k_nearest_neighbors); + void setNKeypoints(int n_keypoints); + void setOutputPath(const std::string& output_path); + void setSACSaveResults(bool sac_save_results); + void setSACReestimateTf(bool sac_reestimate_tf); + void setCandidateGenerationMethod(CandidateGenerationMethod candidate_method); + void setPairwiseMatchingMethod(PairwiseMatchingMethod pairwsie_matching_method); + void setPairwiseMatcherIndex(PairwiseMatcherIndex pairwsie_matcher_index); + + void setMatcherUseDescRatioTest(bool matcher_use_desc_ratio_test); + void setMatcherMaxDescRatio(double matcher_max_desc_ratio); + void setMatcherMaxDescDist(double matcher_max_desc_dist); + + const cv::Mat getAssociationMatrix() const { return association_matrix_; } + const cv::Mat getCandidateMatrix() const { return candidate_matrix_; } + const cv::Mat getCorrespondenceMatrix() const { return correspondence_matrix_; } + const cv::Mat getMatchMatrix() const { return match_matrix_; } + + /** Main method for generating associatuions + * @param keyframes the input vector of RGBD keyframes + * @param associations reference to the output vector of associations + */ + void generateKeyframeAssociations( + KeyframeVector& keyframes, + KeyframeAssociationVector& associations); + + // -------------------------------------------- + + void prepareFeaturesForRANSAC(KeyframeVector& keyframes); + + void buildAssociationMatrix( + const KeyframeVector& keyframes, + KeyframeAssociationVector& associations); + + /** @brief checks the features of the most recently added frame against a subset of keyframes */ + void onlineLoopClosureDetector(KeyframeVector& keyframes, + KeyframeAssociationVector& associations); + + /** @brief Extracts features from a single keyframe */ + void extractFeatures(RGBDKeyframe &keyframe); + + /** @brief Trains matcher from features in a single keyframe */ + void prepareMatcher(KeyframeVector &keyframes); + + private: + + bool verbose_; + + /** @brief Maximim iterations for the RANSAC test + */ + int ransac_max_iterations_; + + /** @brief How many inliers are required to pass the RANSAC test. + * + * If a candidate keyframe has fewer correspondences or more, + * it will not be eligible for a RANSAC test + */ + int sac_min_inliers_; + + bool matcher_use_desc_ratio_test_; + + double matcher_max_desc_ratio_; + + /** @brief Maximum distance (in descriptor space) between + * two features to be considered a correspondence candidate + */ + double matcher_max_desc_dist_; + + /** @brief Maximum distance squared (in Euclidean space) between + * two features to be considered a correspondence candidate + */ + double sac_max_eucl_dist_sq_; + + bool sac_reestimate_tf_; + + double ransac_sufficient_inlier_ratio_; + + double ransac_confidence_; + double log_one_minus_ransac_confidence_; + + /** @brief If true, positive RANSAC results will be saved + * to file as images with keypoint correspondences + */ + bool sac_save_results_; + + /** @brief The path where to save images if sac_save_results_ is true + */ + std::string output_path_; + + /** @brief For kd-tree based correspondences, how many candidate + * keyframes will be tested agains the query keyframe using a RANSAC test + */ + double n_candidates_; + + /** @brief How many nearest neighbors are requested per keypoint + */ + int k_nearest_neighbors_; + + /** @brief Number of desired keypoints to detect in each image + */ + int n_keypoints_; + + double init_surf_threshold_; + + /** @brief TREE of BRUTE_FORCE */ + CandidateGenerationMethod candidate_method_; + + PairwiseMatchingMethod pairwise_matching_method_; + + PairwiseMatcherIndex pairwise_matcher_index_; + + //------------------------------------------ + + /** @brief CV_8UC1, 1 if associated, 0 otherwise */ + cv::Mat association_matrix_; + + /** @brief CV_8UC1, 1 if candidate, 0 otherwise */ + cv::Mat candidate_matrix_; + + /** @brief CV_16UC1, for tree-based matching, contains number of inlier matches */ + cv::Mat correspondence_matrix_; + + /** @brief CV_16UC1, for tree-based matching, contains number of total matches */ + cv::Mat match_matrix_; + + std::vector matchers_; + + cv::FlannBasedMatcher matcher_; + + // ------------ + + void buildCandidateMatrix(const KeyframeVector& keyframes); + + void buildMatchMatrixSurfTree(const KeyframeVector& keyframes); + + void buildCandidateMatrixSurfTree(); + + + + void buildCorrespondenceMatrix( + const KeyframeVector& keyframes, + KeyframeAssociationVector& associations); + + int pairwiseMatching( + int kf_idx_q, int kf_idx_t, + const KeyframeVector& keyframes, + DMatchVector& best_inlier_matches, + Eigen::Matrix4f& best_transformation); + + int pairwiseMatchingBFSAC( + int kf_idx_q, int kf_idx_t, + const KeyframeVector& keyframes, + DMatchVector& best_inlier_matches, + Eigen::Matrix4f& best_transformation); + + int pairwiseMatchingRANSAC( + int kf_idx_q, int kf_idx_t, + const KeyframeVector& keyframes, + DMatchVector& best_inlier_matches, + Eigen::Matrix4f& best_transformation); + + void getCandidateMatches( + const RGBDFrame& frame_q, const RGBDFrame& frame_t, + cv::FlannBasedMatcher& matcher, + DMatchVector& candidate_matches); + + void prepareMatchers( + const KeyframeVector& keyframes); + + cv::FlannBasedMatcher trainMatcher(const RGBDKeyframe& keyframe); + +}; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_KEYFRAME_GRAPH_DETECTOR_H diff --git a/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver.h b/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver.h new file mode 100644 index 0000000..809f6f9 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver.h @@ -0,0 +1,72 @@ +/** + * @file keyframe_graph_solver.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_H +#define RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_H + +#include "rgbdtools/rgbd_keyframe.h" +#include "rgbdtools/graph/keyframe_association.h" + +namespace rgbdtools { + +/** @brief Base class for graph-based global alignment classes + * + * The class takes as input a vector of RGBD keyframes and a + * vector of associations between them, and modifies the keyframe + * poses based on the minimization of some error metric. + */ +class KeyframeGraphSolver +{ + public: + + /** @brief Constructor + */ + KeyframeGraphSolver(); + + /** @brief Default destructor + */ + virtual ~KeyframeGraphSolver(); + + /** @brief Main method to call to perform graph solving. + * + * The keyframe poses will be modified according to some error + * propagation model informed by the graph defined by the keyframe + * associations. + * + * @param keyframes vector of keyframes + * @param associations vector of input keyframe associations + */ + virtual void solve( + KeyframeVector& keyframes, + const KeyframeAssociationVector& associations, + const KeyframeAssociationVector& odometryEdges)=0; + + virtual void solve( + KeyframeVector& keyframes, + const KeyframeAssociationVector& associations, + AffineTransformVector& path) = 0; +}; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_H diff --git a/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver_g2o.h b/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver_g2o.h new file mode 100644 index 0000000..2b7e0d7 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver_g2o.h @@ -0,0 +1,98 @@ +/** + * @file keyframe_graph_solver_g2o.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H +#define RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H + +#include "g2o/core/sparse_optimizer.h" +#include "g2o/core/block_solver.h" +#include "g2o/core/optimization_algorithm_levenberg.h" + + +#include "rgbdtools/graph/keyframe_graph_solver.h" + +namespace rgbdtools { + +/** @brief Graph-based global alignement using g2o (generalized + * graph optimizaiton) + */ +class KeyframeGraphSolverG2O: public KeyframeGraphSolver +{ + public: + + /** @brief Constructor from ROS noehandles + */ + KeyframeGraphSolverG2O(); + + /** @brief Default destructor + */ + ~KeyframeGraphSolverG2O(); + + /** @brief Main method to call to perform graph solving using g2o. + * + * @param keyframes vector of keyframes + * @param associations vector of input keyframe associations + */ + void solve(KeyframeVector& keyframes, const KeyframeAssociationVector & associations, const KeyframeAssociationVector & odometryEdges); + + + void solve(KeyframeVector& keyframes, + const KeyframeAssociationVector& associations, + AffineTransformVector& path); + + private: + + g2o::BlockSolverX::LinearSolverType* linear_solver_; + g2o::BlockSolverX* block_solver_; + g2o::OptimizationAlgorithmLevenberg* optimization_algorithm_; + g2o::SparseOptimizer optimizer_; + + int vertexIdx; + + /** @brief Adds a vertex to the g2o structure + */ + void addVertex(const AffineTransform& vertex_pose, + int vertex_idx); + + /** @brief Adds an edge to the g2o structure + */ + void addEdge(int from_idx, int to_idx, + const AffineTransform& relative_pose, + const Eigen::Matrix& information_matrix); + + /** @brief runs the optimization + */ + void optimizeGraph(); + + /** @brief copies the (optimized) poses from the g2o structure into + * the keyframe vector + * @param keyframes the vector of keyframes to modify the poses + */ + //void updatePoses(KeyframeVector& keyframes); + + void getOptimizedPoses(AffineTransformVector& poses); +}; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H diff --git a/lib_rgbdtools/include/rgbdtools/header.h b/lib_rgbdtools/include/rgbdtools/header.h new file mode 100644 index 0000000..bed0cd2 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/header.h @@ -0,0 +1,19 @@ +#ifndef RGBD_HEADER_H +#define RGBD_HEADER_H + +namespace rgbdtools { + +struct Time { + int32_t sec; + int32_t nsec; +}; + +struct Header { + uint32_t seq; + Time stamp; + std::string frame_id; +}; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_HEADER_H diff --git a/lib_rgbdtools/include/rgbdtools/map_util.h b/lib_rgbdtools/include/rgbdtools/map_util.h new file mode 100644 index 0000000..6d7596d --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/map_util.h @@ -0,0 +1,142 @@ +#ifndef RGBDTOOLS_MAP_UTIL_H +#define RGBDTOOLS_MAP_UTIL_H + +#include +#include +#include +#include +#include + +#include + +#include "rgbdtools/types.h" +#include "rgbdtools/rgbd_keyframe.h" + +namespace rgbdtools { + +void buildExpectedPhiHistorgtam( + cv::Mat& histogram, + double degrees_per_bin, + double stdev); + +void buildPhiHistogram( + const pcl::PointCloud& cloud, + cv::Mat& histogram, + double degrees_per_bin); + +void normalizeHistogram(cv::Mat& histogram); + +void shiftHistogram( + const cv::Mat& hist_in, + cv::Mat& hist_out, + int bins); + +bool alignHistogram( + const cv::Mat& hist, + const cv::Mat& hist_exp, + double hist_resolution, + double& best_angle); + +void create8bitHistogram( + const cv::Mat& histogram, + cv::Mat& histogram_norm); + +void createImageFromHistogram( + const cv::Mat& histogram, + cv::Mat& image); + +void create2DProjectionImage( + const PointCloudT& cloud, + cv::Mat& img, + double min_z = -std::numeric_limits::infinity(), + double max_z = std::numeric_limits::infinity()); + +void filterCloudByHeight( + const pcl::PointCloud& cloud_in, + pcl::PointCloud& cloud_out, + double min_z, + double max_z); + +// *** matching + +/* +void buildDenseAssociationMatrix( + const KeyframeVector& keyframes, + cv::Mat& association_matrix); + +void buildSURFAssociationMatrixBruteForce( + const KeyframeVector& keyframes, + cv::Mat& correspondence_matrix, + int threshold); + + +void buildSURFAssociationMatrixTree( + const KeyframeVector& keyframes, + cv::Mat& correspondence_matrix, + int k_nearest_neighbors, + int n_candidates, + int threshold); + +void buildRANSACCorrespondenceMatrix( + const KeyframeVector& keyframes, + const cv::Mat& candidate_matrix, + cv::Mat& correspondence_matrix); + +void buildSURFCandidateMatrixTree( + const cv::Mat& match_matrix, + cv::Mat& candidate_matrix, + int n_candidates); + +void buildSURFMatchMatrixTree( + const KeyframeVector& keyframes, + cv::Mat& match_matrix, + int k_nearest_neighbors); + +void floatMatrixToUintMatrix( + const cv::Mat& mat_in, + cv::Mat& mat_out, + float scale = 0); + +void prepareFeaturesForRANSAC(KeyframeVector& keyframes); + +void pairwiseMatchingRANSAC( + const RGBDFrame& frame_a, const RGBDFrame& frame_b, + double max_eucl_dist_sq, + double max_desc_dist, + double sufficient_inlier_ratio, + int max_ransac_iterations, + std::vector& all_matches, + std::vector& best_inlier_matches, + Eigen::Matrix4f& best_transformation); +*/ + +void trainSURFMatcher( + const KeyframeVector& keyframes, + cv::FlannBasedMatcher& matcher); + +void getRandomIndices( + int k, int n, IntVector& output); + +void get3RandomIndices( + int n, std::set& mask, IntVector& output); + +double distEuclideanSq(const PointFeature& a, const PointFeature& b); + +void makeSymmetricOR(cv::Mat mat); + +void thresholdMatrix( + const cv::Mat& mat_in, + cv::Mat& mat_out, + int threshold); + +void compareAssociationMatrix( + const cv::Mat& a, + const cv::Mat& b, + int& false_pos, + int& false_neg, + int& total); + + +} // namespace rgbdtools + +#endif // RGBDTOOLS_MAP_UTIL_H diff --git a/lib_rgbdtools/include/rgbdtools/registration/motion_estimation.h b/lib_rgbdtools/include/rgbdtools/registration/motion_estimation.h new file mode 100644 index 0000000..e50a836 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/registration/motion_estimation.h @@ -0,0 +1,111 @@ +/** + * @file motion_estimation.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_MOTION_ESTIMATION_H +#define RGBDTOOLS_MOTION_ESTIMATION_H + +#include + +#include "rgbdtools/rgbd_frame.h" +#include "rgbdtools/types.h" + +namespace rgbdtools { + +/** @brief Base class for visual odometry motion estimation methods + * + * The motion is estimated in increments of the change of pose + * of the base frame. The increments are expressed wrt fixed frame. + */ +class MotionEstimation +{ + public: + + enum MotionConstraint {NONE = 0, ROLL_PITCH = 1, ROLL_PITCH_Z = 2}; + + /** @brief Constructor from ROS nodehandles + * @param nh the public nodehandle + * @param nh_private the private nodehandle + */ + MotionEstimation(); + + /** @brief Default destructor + */ + virtual ~MotionEstimation(); + + /** @brief Main function for estimating motion + * + * The motion is equal to the change of pose of the base frame, and is + * expressed wrt the fixed frame + * + * Pose_new = motion * Pose_old; + * + * @param frame The RGBD Frame for which the motion is estimated + * @return incremental motion transform + */ + AffineTransform getMotionEstimation(RGBDFrame& frame); + + /** @brief Set the transformation between the base and camera frames. + * @param b2c The transform from the base frame to the camera frame, + * expressed wrt the base frame. + */ + void setBaseToCameraTf(const AffineTransform& b2c); + + void setMotionConstraint(int motion_constraint); + + /** @brief Return the size of the internal model. Overriden for classes + * that use a model. + * + * @return the size of the model + */ + virtual int getModelSize() const { return 0; } + + protected: + + AffineTransform b2c_; ///< Base (moving) frame to Camera-optical frame + + int motion_constraint_; ///< The motion constraint type + + /** @brief Implementation of the motion estimation algorithm. + * @param frame the current RGBD frame + * @param prediction the motion prediction (currently ignored) + * @param motion the output motion + * @retval true the motion estimation was successful + * @retval false the motion estimation failed + */ + virtual bool getMotionEstimationImpl( + RGBDFrame& frame, + const AffineTransform& prediction, + AffineTransform& motion) = 0; + + /** @brief Constrains the motion in accordance to the class motion constraints + * + * This method can be called by the inheriting classes if desired. + * + * @param motion the incremental motion which is constrained. + */ + void constrainMotion(AffineTransform& motion); +}; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_MOTION_ESTIMATION_H diff --git a/lib_rgbdtools/include/rgbdtools/registration/motion_estimation_icp_prob_model.h b/lib_rgbdtools/include/rgbdtools/registration/motion_estimation_icp_prob_model.h new file mode 100644 index 0000000..2b51fc8 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/registration/motion_estimation_icp_prob_model.h @@ -0,0 +1,238 @@ +/** + * @file motion_estimation_icp_prob_model.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H +#define RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H + +#include +#include +#include + +#include "rgbdtools/types.h" +#include "rgbdtools/registration/motion_estimation.h" + +namespace rgbdtools { + +/** @brief Motion estimation based on aligning sparse features + * against a persistent, dynamic model. + * + * The model is build from incoming features through a Kalman Filter + * update step. + */ +class MotionEstimationICPProbModel: public MotionEstimation +{ + public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** @brief Constructor from ROS noehandles + * @param nh the public nodehandle + * @param nh_private the private notehandle + */ + MotionEstimationICPProbModel(); + + /** @brief Default destructor + */ + virtual ~MotionEstimationICPProbModel(); + + /** @brief Main method for estimating the motion given an RGBD frame + * @param frame the current RGBD frame + * @param prediction the predicted motion (currently ignored) + * @param motion the (output) incremental motion, wrt the fixed frame + * @retval true the motion estimation was successful + * @retval false the motion estimation failed + */ + bool getMotionEstimationImpl( + RGBDFrame& frame, + const AffineTransform& prediction, + AffineTransform& motion); + + /** @brief Returns the number of points in the model built from the feature buffer + * @returns number of points in model + */ + int getModelSize() const { return model_size_; } + + PointCloudFeature::Ptr getModel() { return model_ptr_; } + Vector3fVector* getMeans() { return &means_; } + Matrix3fVector* getCovariances() { return &covariances_; } + + void setMaxIterations(int max_iterations); + void setMinCorrespondences(int min_correspondences); + void setNNearestNeighbors(int n_nearest_neighbors); + void setMaxModelSize(int max_model_size); + void setTfEpsilonLinear(double tf_epsilon_linear); + void setTfEpsilonAngular(double tf_epsilon_angular); + void setMaxAssociationDistMahalanobis(double max_assoc_dist_mah); + void setMaxCorrespondenceDistEuclidean(double max_corresp_dist_eucl); + + private: + + // **** params + + int max_iterations_; ///< Maximum number of ICP iterations + int min_correspondences_; ///< Minimum number of correspondences for ICP to contuinue + + /** @brief How many euclidean neighbors to go through, in a brute force + * search of the closest Mahalanobis neighbor. + */ + int n_nearest_neighbors_; + + /** @brief Upper bound for how many features to store in the model. + * + * New features added beyond thi spoint will overwrite old features + */ + int max_model_size_; + + double tf_epsilon_linear_; ///< Linear convergence criteria for ICP + double tf_epsilon_angular_; ///< Angular convergence criteria for ICP + + /** @brief Maximum Mahalanobis distance for associating points + * between the data and the model + */ + double max_assoc_dist_mah_; + + /** @brief Maximum Euclidean correspondce distance for ICP + */ + double max_corresp_dist_eucl_; + + + /** @brief Maximum squared Mahalanobis distance for associating points + * between the data and the model, derived. + */ + double max_assoc_dist_mah_sq_; + + /** @brief Maximum Euclidean correspondce distance for ICP, derived + */ + double max_corresp_dist_eucl_sq_; + + // **** variables + + PointCloudFeature::Ptr model_ptr_; ///< The model point cloud + int model_idx_; ///< Current intex in the ring buffer + int model_size_; ///< Current model size + Vector3fVector means_; ///< Vector of model feature mean + Matrix3fVector covariances_; ///< Vector of model feature covariances + + KdTree model_tree_; ///< Kdtree of model_ptr_ + + Matrix3f I_; ///< 3x3 Identity matrix + + AffineTransform f2b_; ///< Transform from fixed to moving frame + + // ***** funtions + + /** @brief Performs ICP alignment using the Euclidean distance for corresopndences + * @param data_means a vector of 3x1 matrices, repesenting the 3D positions of the features + * @param correction reference to the resulting transformation + * @retval true the motion estimation was successful + * @retval false the motion estimation failed + */ + bool alignICPEuclidean( + const Vector3fVector& data_means, + AffineTransform& correction); + + /** @brief Performs ICP alignment using the Euclidean distance for corresopndences + * @param data_cloud a pointcloud of the 3D positions of the features + * @param data_indices reference to a vector containting the resulting data indices + * @param model_indices reference to a vector containting the resulting model indices + */ + void getCorrespEuclidean( + const PointCloudFeature& data_cloud, + IntVector& data_indices, + IntVector& model_indices); + + /** @brief Finds the nearest Euclidean neighbor + * @param data_point the query 3D data point + * @param eucl_nn_idx reference to the resulting nearest neigbor index in the model + * @param eucl_dist_sq reference to the resulting squared distance + * @retval true a neighbor was found + * @retval false a neighbor was not found + */ + bool getNNEuclidean( + const PointFeature& data_point, + int& eucl_nn_idx, double& eucl_dist_sq); + + /** @brief Finds the nearest Mahalanobis neighbor + * + * Requests the K nearest Euclidean neighbors (K = n_nearest_neighbors_) + * using a kdtree, and performs a brute force search for the closest + * Mahalanobis nighbor. Reasonable values for K are 4 or 8. + * + * @param data_mean 3x1 matrix of the query 3D data point mean + * @param data_cov 3x3 matrix of the query 3D data point covariance + * @param mah_nn_idx reference to the resulting nearest neigbor index in the model + * @param mah_dist_sq reference to the resulting squared Mahalanobis distance + * @param indices cache vector, pre-allocated with n_nearest_neighbors_ size + * @param dists_sq cache vector, pre-allocated with n_nearest_neighbors_ size + * @retval true a neighbor was found + * @retval false a neighbor was not found + */ + bool getNNMahalanobis( + const Vector3f& data_mean, const Matrix3f& data_cov, + int& mah_nn_idx, double& mah_dist_sq, + IntVector& indices, FloatVector& dists_sq); + + /** @brief Initializes the (empty) model from a set of data means and + * covariances + * @param data_means vector of 3x1 data point means + * @param data_covariances vector of 3x3 data point covariances + */ + void initializeModelFromData( + const Vector3fVector& data_means, + const Matrix3fVector& data_covariances); + + /** @brief Updates the (non-empty) model from a set of data means and + * covariances + * + * Any data points which have a Mahalanobis neighbor in the model + * under a certain threshold distance get updated using a Kalman filter. + * + * Any data points without a Mahalanobis neighbor get insterted as new + * model points. + * + * @param data_means vector of 3x1 data point means + * @param data_covariances vector of 3x3 data point covariances + */ + void updateModelFromData(const Vector3fVector& data_means, + const Matrix3fVector& data_covariances); + + /** @brief Adds a new point to the model + * + * The model is implemented using a rign buffer. If the number of + * points in the model has reached the maximum, the new point will + * overwrite the oldest model point + * + * @param data_mean 3x1 data point means + * @param data_cov 3x3 data point covariances + */ + void addToModel( + const Vector3f& data_mean, + const Matrix3f& data_cov); + + bool saveModel(const std::string& filename); +}; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_MOTION_ESTIMATION_ICP_PROB_MODEL_H + diff --git a/lib_rgbdtools/include/rgbdtools/rgbd_frame.h b/lib_rgbdtools/include/rgbdtools/rgbd_frame.h new file mode 100644 index 0000000..cb3afcf --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/rgbd_frame.h @@ -0,0 +1,203 @@ +/** + * @file rgbd_frame.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_RGBD_FRAME_H +#define RGBDTOOLS_RGBD_FRAME_H + +#include +#include +#include +#include + +#include "rgbdtools/types.h" +#include "rgbdtools/header.h" +#include "rgbdtools/rgbd_util.h" + +namespace rgbdtools { + +/** @brief Auxiliarry class that holds together rgb and depth images. + * + * The class also holds the detected keypoints and their 3D distributions. + * Keypoints, descriptos, and kp_* are indexed the same way and may include + * invalid points. An invalid keypoint occurs when: + * - no z data + * - z > threshold + * - var_z > threshold + */ +class RGBDFrame +{ + public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** @brief Default (empty) constructor. + */ + RGBDFrame(); + + /** @brief Constructor from ROS messages + * @param rgb_img_in 8UC3 image message + * @param depth_img_in 16UC1 ROS depth message (in mm, 0 = invalid data) + * @param info_msg ROS camera info message, assumed no distortion, applies to both images + */ + + RGBDFrame(const cv::Mat& rgb_img_in, + const cv::Mat& depth_img_in, + const cv::Mat& intr_in, + const Header& header_in); + + int index; + + Header header; ///< Header taken from rgb_msg + cv::Mat rgb_img; ///< RGB image (8UC3) + cv::Mat depth_img; ///< Depth image in mm (16UC1). 0 = invalid data + cv::Mat intr; + + /** @brief The intrinsic matrix which applies to both images. + * + * It's assumed that the images are already + * rectified and in the same camera frame(RGB) + */ + //image_geometry::PinholeCameraModel model; + + KeypointVector keypoints; ///< 2D keypoint locations + cv::Mat descriptors; ///< Feature descriptor vectors + + BoolVector kp_valid; ///< Is the z data valid? + Vector3fVector kp_means; ///< 1x3 mat of 3D locations + Matrix3fVector kp_covariances; ///< 3x3 mat of covariances + + int n_valid_keypoints; ///< How many keypoints have usable 3D data + + /** @brief Computes the 3D means and covariances for all the detected keypoints. + * + * Some features will be marked as invalid. + * @todo do we want default values? + * @param max_z [m] features with z bigger than this will be marked as invalid + * @param max_stdev_z [m] features with std_dev(z) bigger than this + * will be marked as invalid + */ + void computeDistributions( + double max_z = 5.5, + double max_stdev_z = 0.03); + + /** @brief Computes the 3D means and covariances for all the valid detected keypoints. + * @param cloud Reference to the point cloud to be constructed + */ + void constructFeaturePointCloud(PointCloudFeature& cloud); + + /** @brief constructs a point cloud from the RGB and depth images + * @param cloud the reference to the point cloud to be constructed + * @param max_z [m] points with z bigger than this will be marked as NaN + * @param max_stdev_z [m] points with std_dev(z) bigger than this + * will be marked as NaN + * + * @todo do we want default values? or ROS parameters here) + * + */ + void constructDensePointCloud(PointCloudT& cloud, + double max_z = 5.5, + double max_stdev_z = 0.03) const; + + /** @brief Saves the RGBD frame to disk. + * + * Saves the RGB and depth images as png, and the header and intrinsic matrix + * as .yml files. Parent directory must exist prior to calling this function, or + * function will fail. + * + * @param frame Reference to the frame being saved + * @param path The path to the folder where everything will be stored + * + * @retval true Successfully saved the data + * @retval false Saving failed - for example, cannot create directory + */ + static bool save(const RGBDFrame& frame, const std::string& path); + + /** @brief Loads the RGBD frame from disk. + * + * Loands the RGB and depth images from png, and the header and intrinsic matrix + * from .yml files. + * + * @param frame Reference to the frame being loaded + * @param path The path to the folder where everything was saved. + * + * @retval true Successfully loaded the data + * @retval false Loading failed - for example, directory not found + */ + static bool load(RGBDFrame& frame, const std::string& path); + + protected: + + /** @brief Constant for calculating std_dev(z) + * + * Khoshelham, K.; Elberink, S.O. Accuracy and Resolution of Kinect + * Depth Data for Indoor Mapping Applications. Sensors 2012, 12, 1437-1454. + */ + static const double Z_STDEV_CONSTANT = 0.001425; + + /** @brief Calculates the var(z) from z + * @param z the z depth, in meters + * @return the variance in z, in meters^2 + */ + double getVarZ(double z) const; + + /** @brief Calculates the std_dev(z) from z + * @param z the z depth, in meters + * @return the standard deviation in z, in meters + */ + double getStdDevZ(double z) const; + + /** @brief Calculates the z distribution (mean and variance) for a given pixel + * + * Calculation is based on the standard quadratic model. See: + * + * Khoshelham, K.; Elberink, S.O. Accuracy and Resolution of Kinect + * Depth Data for Indoor Mapping Applications. Sensors 2012, 12, 1437-1454. + * + * @param u the pixel u-coordinate + * @param v the pixel v-coordinate + * @param z_mean the mean of the distribution (will be the same az the z of the pixel), in meters + * @param z_var var(z), will a quadratic function of the mean, in meters^2 + */ + void getGaussianDistribution(int u, int v, double& z_mean, double& z_var) const; + + /** @brief Calculates the GMM z distribution (mean and variance) for a given pixel + * + * Calculation is based on a Gaussian Mixture Model on top of + * the standard quadratic model. The neigboring pixels contribute different + * wights to the mixture. See: + * + * Dryanovski et al ICRA2013 papaer + * + * @todo reference for the paper + * + * @param u the pixel u-coordinate + * @param v the pixel v-coordinate + * @param z_mean the mean of the distribution (will be the same az the z of the pixel), in meters + * @param z_var var(z), will a quadratic function of the mean, in meters^2 + */ + void getGaussianMixtureDistribution(int u, int v, double& z_mean, double& z_var) const; +}; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_RGBD_FRAME_H diff --git a/lib_rgbdtools/include/rgbdtools/rgbd_keyframe.h b/lib_rgbdtools/include/rgbdtools/rgbd_keyframe.h new file mode 100644 index 0000000..83ce463 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/rgbd_keyframe.h @@ -0,0 +1,122 @@ +/** + * @file rgbd_keyframe.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_RGBD_KEYFRAME_H +#define RGBDTOOLS_RGBD_KEYFRAME_H + +#include + +#include "rgbdtools/rgbd_frame.h" + +namespace rgbdtools { + +/** @brief Extension of an RGBDFrame, which has a pose, and a 3D point cloud + * + * The class is used for keyframe-based graph alignment, as well as dense + * mapping. + */ + +class RGBDKeyframe: public RGBDFrame +{ + public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** @brief Default (empty) constructor. + */ + RGBDKeyframe(); + + /** @brief Copy constructor from a RGBDFrame + * @param frame reference to the frame which is being used to create a keyframe + */ + RGBDKeyframe(const RGBDFrame& frame); + + Pose pose; ///< pose of the camera, in some fixed frame + + bool manually_added; ///< Whether the frame was added manually by the user + + /** @brief path length, in meters, of the camera trajectory at the moment + * of adding the keyframe + */ + double path_length_linear; + + /** @brief path length, in radians, of the camera trajectory at the + * moment of adding the keyframe + */ + double path_length_angular; + + /** @brief Saves the RGBD keyframe to disk. + * + * Saves the RGBDFrame, as well as the additional keyframe info as .yml + * + * @param keyframe Reference to the keyframe being saved + * @param path The path to the folder where everything will be stored + * + * @retval true Successfully saved the data + * @retval false Saving failed - for example, cannot create directory + */ + static bool save(const RGBDKeyframe& keyframe, + const std::string& path); + + /** @brief Loads the RGBD keyframe to disk. + * + * Loads the RGBDFrame, as well as the additional keyframe info as .yml + * + * @param keyframe Reference to the keyframe being saved + * @param path The path to the folder where everything will be stored + * + * @retval true Successfully loaded the data + * @retval false Loading failed - for example, directory not found + */ + static bool load(RGBDKeyframe& keyframe, + const std::string& path); +}; + +typedef Eigen::aligned_allocator KeyframeAllocator; +typedef std::vector KeyframeVector; + +/** @brief Saves a vector of RGBD keyframes to disk. +* +* @param keyframes Reference to the keyframe being saved +* @param path The path to the folder where everything will be stored +* +* @retval true Successfully saved the data +* @retval false Saving failed - for example, cannot create directory +*/ +bool saveKeyframes(const KeyframeVector& keyframes, + const std::string& path); + +/** @brief Loads a vector of RGBD keyframes to disk. +* +* @param keyframes Reference to the keyframe being saved +* @param path The path to the folder where everything will be stored +* +* @retval true Successfully saved the data +* @retval false Saving failed - for example, cannot create directory +*/ +bool loadKeyframes(KeyframeVector& keyframes, + const std::string& path); + +} // namespace rgbdtools + +#endif // RGBDTOOLS_RGBD_KEYFRAME_H diff --git a/lib_rgbdtools/include/rgbdtools/rgbd_util.h b/lib_rgbdtools/include/rgbdtools/rgbd_util.h new file mode 100644 index 0000000..8a99604 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/rgbd_util.h @@ -0,0 +1,217 @@ +/** + * @file rgbd_util.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_RGBD_UTIL_H +#define RGBDTOOLS_RGBD_UTIL_H + +#include +#include +#include +#include + +#include "rgbdtools/types.h" + +namespace rgbdtools { + +/** @brief Polynomial fit modes for depth unwarping + * + * The modes include: + * - DEPTH_FIT_LINEAR (c0 + c1*d) + * - DEPTH_FIT_LINEAR_ZERO (c1*d) + * - DEPTH_FIT_QUADRATIC (c0 + c1*d + c2*d^2) + * - DEPTH_FIT_QUADRATIC_ZERO (c1*d + c2*d^2) + * + * The recommended mode is DEPTH_FIT_QUADRATIC + */ +enum DepthFitMode { + DEPTH_FIT_LINEAR, + DEPTH_FIT_LINEAR_ZERO, + DEPTH_FIT_QUADRATIC, + DEPTH_FIT_QUADRATIC_ZERO +}; + +/** @brief Filters out a vector of means given a mask of valid + * entries + * + * @param means input vector of 3x1 matrices + * @param valid vector mask of valid flags + * @param means_f output vector of 3x1 matrices + */ +void removeInvalidMeans( + const Vector3fVector& means, + const BoolVector& valid, + Vector3fVector& means_f); + +/** @brief Filters out a vector of means and a vector of + * covariances given a mask of valid entries + * + * @param means input vector of 3x1 matrices + * @param covariances input vector of 3x3 matrices + * @param valid vector mask of valid flags + * @param means_f output vector of 3x1 matrices + * @param covariances_f output vector of 3x1 matrices + */ +void removeInvalidDistributions( + const Vector3fVector& means, + const Matrix3fVector& covariances, + const BoolVector& valid, + Vector3fVector& means_f, + Matrix3fVector& covariances_f); + +/** @brief Transforms a vector of means + * + * @param means vector of 3x1 matrices of positions (3D means) + * @param transform the tranformation to be applied to all the means + */ +void transformMeans( + Vector3fVector& means, + const AffineTransform& transform); + +/** @brief Transforms a vector of means and covariances + * + * @param means vector of 3x1 matrices of positions (3D means) + * @param covariances vector of 3x3 covariance matrices + * @param transform the transformation to be applied to all the means and covariances + */ +void transformDistributions( + Vector3fVector& means, + Matrix3fVector& covariances, + const AffineTransform& transform); + +/** @brief Creates a pcl point cloud form a vector + * of eigen matrix means + * + * @param means vector of 3x1 matrices of positions (3D means) + * @param cloud reference to the output cloud + */ +void pointCloudFromMeans( + const Vector3fVector& means, + PointCloudFeature& cloud); + +/** @brief reprojects a depth image to another depth image, + * registered in the rgb camera's frame. + * + * Both images need to be rectified first. ir2rgb is a matrix + * such that for any point P_IR in the depth camera frame + * P_RGB = ir2rgb * P_IR + * + * @param intr_rect_ir intrinsic matrix of the rectified depth image + * @param intr_rect_rgb intrinsic matrix of the rectified RGB image + * @param ir2rgb extrinsic matrix between the IR(depth) and RGB cameras + * @param depth_img_rect the input image: rectified depth image + * @param depth_img_rect_reg the output image: rectified and registered into the + * RGB frame + */ +void buildRegisteredDepthImage( + const cv::Mat& intr_rect_ir, + const cv::Mat& intr_rect_rgb, + const cv::Mat& ir2rgb, + const cv::Mat& depth_img_rect, + cv::Mat& depth_img_rect_reg); + +/** @brief Constructs a point cloud, a depth image and intrinsic matrix + * + * @param depth_img_rect rectified depth image (16UC1, in mm) + * @param intr_rect_ir intinsic matrix of the rectified depth image + * @param cloud reference to teh output point cloud + */ +void buildPointCloud( + const cv::Mat& depth_img_rect, + const cv::Mat& intr_rect_ir, + PointCloudT& cloud); + +/** @brief Constructs a point cloud with color + * + * Prior to calling this functions, both images need to be rectified, + * and the depth image has to be registered into the frame of the RGB image. + * + * @param depth_img_rect_reg rectified and registered depth image (16UC1, in mm) + * @param rgb_img_rect rectified rgb image (8UC3) + * @param intr_rect_rgb intrinsic matrix + * @param cloud reference to the output point cloud + */ +void buildPointCloud( + const cv::Mat& depth_img_rect_reg, + const cv::Mat& rgb_img_rect, + const cv::Mat& intr_rect_rgb, + PointCloudT& cloud); + +/** @brief converts a 32FC1 depth image (in meters) to a + * 16UC1 depth image (in mm). + * + * @param depth_image_in the input 32FC1 image + * @param depth_image_out the output 16UC1 image + */ +void depthImageFloatTo16bit( + const cv::Mat& depth_image_in, + cv::Mat& depth_image_out); + +void eigenAffineToOpenCVRt( + const AffineTransform& transform, + cv::Mat& R, + cv::Mat& t); + +void openCVRtToEigenAffine( + const cv::Mat& R, + const cv::Mat& t, + AffineTransform& eigen_affine); + +void eigenAffineToXYZRPY( + const AffineTransform& transform, + float& x, float& y, float& z, + float& roll, float& pitch, float& yaw); + +void XYZRPYToEigenAffine( + float x, float y, float z, + float roll, float pitch, float yaw, + AffineTransform& t); + +void getTfDifference( + const AffineTransform& transform, + double& dist, double& angle); + +/** @brief Given a depth image, uwarps it according to a polynomial model + * + * The size of the c matrices should be equal to the image size. + * + * @param depth_img_in depth image to be unwarped (16UC1, in mm) + * @param coeff0 matrix of c0 coefficients + * @param coeff1 matrix of c1 coefficients + * @param coeff2 matrix of c2 coefficients + * @param fit_mode the polynomial fitting mode, see \ref DepthFitMode. + * d = c0 + c1*d + c2*d^2 + */ +void unwarpDepthImage( + cv::Mat& depth_img_in, + const cv::Mat& coeff0, + const cv::Mat& coeff1, + const cv::Mat& coeff2, + int fit_mode=DEPTH_FIT_QUADRATIC); + +void setRPY( + float roll, float pitch, float yaw, + Matrix3f& m); + +} // namespace rgbdtools + +#endif // RGBDTOOLS_RGBD_UTIL_H diff --git a/lib_rgbdtools/include/rgbdtools/rgbdtools.h b/lib_rgbdtools/include/rgbdtools/rgbdtools.h new file mode 100644 index 0000000..9146cad --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/rgbdtools.h @@ -0,0 +1,49 @@ +/** + * @file rgbdtools.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_RGBDTOOLS_H +#define RGBDTOOLS_RGBDTOOLS_H + +#include "rgbdtools/types.h" +#include "rgbdtools/rgbd_util.h" +#include "rgbdtools/map_util.h" + +#include "rgbdtools/header.h" +#include "rgbdtools/rgbd_frame.h" +#include "rgbdtools/rgbd_keyframe.h" + +#include "rgbdtools/features/feature_detector.h" +#include "rgbdtools/features/gft_detector.h" +#include "rgbdtools/features/orb_detector.h" +#include "rgbdtools/features/star_detector.h" +#include "rgbdtools/features/surf_detector.h" + +#include "rgbdtools/registration/motion_estimation.h" +#include "rgbdtools/registration/motion_estimation_icp_prob_model.h" + +#include "rgbdtools/graph/keyframe_association.h" +#include "rgbdtools/graph/keyframe_graph_detector.h" +#include "rgbdtools/graph/keyframe_graph_solver.h" +#include "rgbdtools/graph/keyframe_graph_solver_g2o.h" + +#endif // RGBDTOOLS_RGBDTOOLS_H diff --git a/lib_rgbdtools/include/rgbdtools/types.h b/lib_rgbdtools/include/rgbdtools/types.h new file mode 100644 index 0000000..4b1a763 --- /dev/null +++ b/lib_rgbdtools/include/rgbdtools/types.h @@ -0,0 +1,72 @@ +/** + * @file types.h + * @author Ivan Dryanovski + * + * @section LICENSE + * + * Copyright (C) 2013, City University of New York + * CCNY Robotics Lab + * + * This program 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. + * + * This program 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 this program. If not, see . + */ + +#ifndef RGBDTOOLS_TYPES_H +#define RGBDTOOLS_TYPES_H + +#include +#include +#include +#include +#include +#include + +namespace rgbdtools { + +// Eigen matrix types + +typedef Eigen::Matrix3f Matrix3f; +typedef Eigen::Vector3f Vector3f; +typedef Eigen::Affine3f Pose; +typedef Eigen::Affine3f AffineTransform; +typedef Eigen::Matrix InformationMatrix; + +// Vector types + +typedef std::vector IntVector; +typedef std::vector FloatVector; +typedef std::vector BoolVector; +typedef std::vector Point2fVector; +typedef std::vector Point3fVector; +typedef std::vector Matrix3fVector; +typedef std::vector Vector3fVector; +typedef std::vector KeypointVector; +typedef std::vector DMatchVector; + +typedef Eigen::aligned_allocator AffineTransformAllocator; +typedef std::vector AffineTransformVector; + +// PCL types + +typedef pcl::PointXYZRGB PointT; +typedef pcl::PointCloud PointCloudT; + +typedef pcl::PointXYZ PointFeature; +typedef pcl::PointCloud PointCloudFeature; + +typedef pcl::KdTreeFLANN KdTree; +typedef pcl::registration::TransformationEstimationSVD TransformationEstimationSVD; + +} // namespace rgbdtools + +#endif // RGBDTOOLS_TYPES_H diff --git a/lib_rgbdtools/rgbdtools b/lib_rgbdtools/rgbdtools new file mode 100644 index 0000000..e69de29 diff --git a/lib_rgbdtools/rgbdtools_git b/lib_rgbdtools/rgbdtools_git new file mode 160000 index 0000000..ed16d35 --- /dev/null +++ b/lib_rgbdtools/rgbdtools_git @@ -0,0 +1 @@ +Subproject commit ed16d350cb18181b3d032b9fba8b41d20cc20105 diff --git a/lib_rgbdtools/rospack_nosubdirs b/lib_rgbdtools/rospack_nosubdirs new file mode 100644 index 0000000..e69de29 From eac0ffff4d5133a53fb08075177a180fb114a654 Mon Sep 17 00:00:00 2001 From: Erik Rosen Date: Mon, 20 Jan 2014 23:23:10 +0000 Subject: [PATCH 05/11] Recommit --- .gitmodules | 3 +++ lib_rgbdtools/rgbdtools_git | 1 - 2 files changed, 3 insertions(+), 1 deletion(-) create mode 100644 .gitmodules delete mode 160000 lib_rgbdtools/rgbdtools_git diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..d61a44b --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "lib_rgbdtools/rgbdtools_git"] + path = lib_rgbdtools/rgbdtools_git + url = https://github.com/ccny-ros-pkg/rgbdtools.git diff --git a/lib_rgbdtools/rgbdtools_git b/lib_rgbdtools/rgbdtools_git deleted file mode 160000 index ed16d35..0000000 --- a/lib_rgbdtools/rgbdtools_git +++ /dev/null @@ -1 +0,0 @@ -Subproject commit ed16d350cb18181b3d032b9fba8b41d20cc20105 From dde615311a900558b486070869158a394f89d57d Mon Sep 17 00:00:00 2001 From: Erik Rosen Date: Sun, 26 Jan 2014 15:46:12 +0000 Subject: [PATCH 06/11] Realtime graph slam working --- .../include/ccny_rgbd/apps/keyframe_mapper.h | 11 +++ ccny_rgbd/src/apps/keyframe_mapper.cpp | 79 +++++++++++++++---- .../rgbdtools/graph/keyframe_graph_detector.h | 14 +++- 3 files changed, 86 insertions(+), 18 deletions(-) diff --git a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h index fb829fa..9aee83a 100644 --- a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h +++ b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -173,6 +174,10 @@ class KeyframeMapper bool solveGraphSrvCallback( SolveGraph::Request& request, SolveGraph::Response& response); + + /** @brief Publishes a voxel grid filtered point cloud map from the current keyframe positions + */ + void publishMap(void); protected: @@ -252,6 +257,10 @@ class KeyframeMapper /** @brief Camera info message subscriber */ CameraInfoSubFilter sub_info_; + + ros::Subscriber odom_sub_; + float angularVelocity_; + void updateOdom(const nav_msgs::Odometry & odom_msg); // params double pcd_map_res_; ///< downsampling resolution of pcd map (in meters) @@ -276,6 +285,8 @@ class KeyframeMapper PathMsg path_msg_; /// < contains a vector of positions of the camera (not base) pose AffineTransform aggregatedPoseCorrection_; + + std::vector uncorrected_keyframe_poses_; /** @brief processes an incoming RGBD frame with a given pose, * and determines whether a keyframe should be inserted diff --git a/ccny_rgbd/src/apps/keyframe_mapper.cpp b/ccny_rgbd/src/apps/keyframe_mapper.cpp index e60c5a1..8978281 100644 --- a/ccny_rgbd/src/apps/keyframe_mapper.cpp +++ b/ccny_rgbd/src/apps/keyframe_mapper.cpp @@ -80,6 +80,8 @@ KeyframeMapper::KeyframeMapper( sub_depth_.subscribe(depth_it, "/rgbd/depth", queue_size_); sub_info_.subscribe(nh_, "/rgbd/info", queue_size_); + odom_sub_ = nh_.subscribe("/odom", 1, &KeyframeMapper::updateOdom,this); + // Synchronize inputs. sync_.reset(new RGBDSynchronizer3( RGBDSyncPolicy3(queue_size_), sub_rgb_, sub_depth_, sub_info_)); @@ -127,6 +129,7 @@ void KeyframeMapper::initParams() int graph_n_keypoints; int graph_n_candidates; int graph_k_nearest_neighbors; + int graph_min_temporal_distance; bool graph_matcher_use_desc_ratio_test = true; if (!nh_private_.getParam ("graph/n_keypoints", graph_n_keypoints)) @@ -135,11 +138,14 @@ void KeyframeMapper::initParams() graph_n_candidates = 15; if (!nh_private_.getParam ("graph/k_nearest_neighbors", graph_k_nearest_neighbors)) graph_k_nearest_neighbors = 4; + if (!nh_private_.getParam ("graph/min_temporal_distance_for_RANSAC", graph_min_temporal_distance)) + graph_min_temporal_distance = 2; graph_detector_.setNKeypoints(graph_n_keypoints); graph_detector_.setNCandidates(graph_n_candidates); graph_detector_.setKNearestNeighbors(graph_k_nearest_neighbors); graph_detector_.setMatcherUseDescRatioTest(graph_matcher_use_desc_ratio_test); + graph_detector_.setRANSACMinTemporalDistance(graph_min_temporal_distance); graph_detector_.setSACReestimateTf(false); graph_detector_.setSACSaveResults(false); @@ -147,6 +153,11 @@ void KeyframeMapper::initParams() aggregatedPoseCorrection_.setIdentity(); } + +void KeyframeMapper::updateOdom(const nav_msgs::Odometry & odom_msg){ + angularVelocity_ = odom_msg.twist.twist.angular.z; + //printf("Angular velocity %f \n", angularVelocity_); +} void KeyframeMapper::RGBDCallback( const ImageMsg::ConstPtr& rgb_msg, @@ -189,7 +200,9 @@ bool KeyframeMapper::processFrame( { // add the frame pose to the path vector geometry_msgs::PoseStamped frame_pose; - tf::Transform frame_tf = tfFromEigenAffine(pose); + + //Correct the pose! + tf::Transform frame_tf = tfFromEigenAffine(aggregatedPoseCorrection_ * pose); tf::poseTFToMsg(frame_tf, frame_pose.pose); // update the header of the pose for the path @@ -210,16 +223,27 @@ bool KeyframeMapper::processFrame( else { double dist, angle; - getTfDifference(tfFromEigenAffine(pose), + getTfDifference(tfFromEigenAffine(aggregatedPoseCorrection_ *pose), tfFromEigenAffine(keyframes_.back().pose), dist, angle); - if (dist > kf_dist_eps_ || angle > kf_angle_eps_) + if (dist > kf_dist_eps_ || angle > kf_angle_eps_){ result = true; - else + } + else{ result = false; + } } + if(fabs(angularVelocity_)>0.2){ + result = false; + printf("Angular velocity too large! Will not add keyframe \n"); + } + else{ + printf("Angular velocity: %f \n", angularVelocity_); + } + + if (result) { addKeyframe(frame, pose); @@ -232,7 +256,9 @@ void KeyframeMapper::addKeyframe( const AffineTransform& pose) { rgbdtools::RGBDKeyframe keyframe(frame); - keyframe.pose = pose; + //Apply correction to pose + keyframe.pose = aggregatedPoseCorrection_ * pose; + uncorrected_keyframe_poses_.push_back(pose); int associations_prev = associations_.size(); if (manual_add_) @@ -258,8 +284,8 @@ void KeyframeMapper::addKeyframe( int from_idx = keyframes_.size() - 2; int to_idx = keyframes_.size() - 1; - const AffineTransform& from_pose = keyframes_[from_idx].pose; - const AffineTransform& to_pose = keyframes_[to_idx].pose; + const AffineTransform& from_pose = uncorrected_keyframe_poses_[from_idx]; + const AffineTransform& to_pose = uncorrected_keyframe_poses_[to_idx]; AffineTransform tf = from_pose.inverse() * to_pose; odometryEdge.kf_idx_a = from_idx; @@ -284,8 +310,8 @@ void KeyframeMapper::addKeyframe( //Calculate and publish pose correction AffineTransform poseCorrection; - poseCorrection = pose.inverse() * keyframes_[keyframes_.size()-1].pose; - aggregatedPoseCorrection_=poseCorrection * aggregatedPoseCorrection_; + poseCorrection = keyframes_[keyframes_.size()-1].pose * uncorrected_keyframe_poses_[uncorrected_keyframe_poses_.size()-1].inverse() ; + aggregatedPoseCorrection_=poseCorrection; publishAggregatedPoseCorrection(); } @@ -324,7 +350,7 @@ bool KeyframeMapper::publishKeyframesSrvCallback( PublishKeyframes::Request& request, PublishKeyframes::Response& response) { - bool found_match = false; + /*bool found_match = false; // regex matching - try match the request string against each // keyframe index @@ -350,7 +376,9 @@ bool KeyframeMapper::publishKeyframesSrvCallback( publishPath(); - return found_match; + return found_match;*/ + publishMap(); + return true; } void KeyframeMapper::publishKeyframeData(int i) @@ -370,6 +398,14 @@ void KeyframeMapper::publishKeyframeData(int i) keyframes_pub_.publish(cloud_ff); } +void KeyframeMapper::publishMap(void) +{ + PointCloudT pcd_map; + buildPcdMap(pcd_map); + + keyframes_pub_.publish(pcd_map); +} + void KeyframeMapper::publishKeyframeAssociations() { visualization_msgs::Marker marker; @@ -408,9 +444,9 @@ void KeyframeMapper::publishKeyframeAssociations() marker.points[idx_end].y = keyframe_b_pose.getOrigin().getY(); marker.points[idx_end].z = keyframe_b_pose.getOrigin().getZ(); - if (association.type == rgbdtools::KeyframeAssociation::VO) + if (association.type == rgbdtools::KeyframeAssociation::ODOMETRY) { - marker.ns = "VO"; + marker.ns = "ODOMETRY"; marker.scale.x = 0.002; marker.color.r = 0.0; @@ -751,14 +787,23 @@ void KeyframeMapper::buildPcdMap(PointCloudT& map_cloud) { const rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx]; - PointCloudT cloud; - keyframe.constructDensePointCloud(cloud, max_range_, max_stdev_); - + PointCloudT::Ptr cloud(new PointCloudT()); + keyframe.constructDensePointCloud(*cloud, max_range_, max_stdev_); + + pcl::VoxelGrid sor; + sor.setInputCloud(cloud); + sor.setLeafSize(pcd_map_res_, pcd_map_res_, pcd_map_res_); + sor.setFilterFieldName("z"); + sor.setFilterLimits (-std::numeric_limits::infinity(), max_map_z_); + PointCloudT cloud_filtered; + sor.filter(cloud_filtered); PointCloudT cloud_tf; - pcl::transformPointCloud(cloud, cloud_tf, keyframe.pose); + + pcl::transformPointCloud(cloud_filtered, cloud_tf, keyframe.pose); cloud_tf.header.frame_id = fixed_frame_; *aggregate_cloud += cloud_tf; + } // filter cloud using voxel grid, and for max z diff --git a/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_detector.h b/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_detector.h index 659becc..a244058 100644 --- a/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_detector.h +++ b/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_detector.h @@ -81,6 +81,7 @@ class KeyframeGraphDetector void setCandidateGenerationMethod(CandidateGenerationMethod candidate_method); void setPairwiseMatchingMethod(PairwiseMatchingMethod pairwsie_matching_method); void setPairwiseMatcherIndex(PairwiseMatcherIndex pairwsie_matcher_index); + void setRANSACMinTemporalDistance(int graph_min_temporal_distance); void setMatcherUseDescRatioTest(bool matcher_use_desc_ratio_test); void setMatcherMaxDescRatio(double matcher_max_desc_ratio); @@ -118,6 +119,10 @@ class KeyframeGraphDetector void prepareMatcher(KeyframeVector &keyframes); private: + double surf_threshold; + int edot; + int eintegral; + int e; bool verbose_; @@ -176,6 +181,12 @@ class KeyframeGraphDetector int n_keypoints_; double init_surf_threshold_; + + int RANSAC_min_temporal_distance_; + + bool motion_constraint_; + + void constrainMotion(Eigen::Matrix4f& motionEig); /** @brief TREE of BRUTE_FORCE */ CandidateGenerationMethod candidate_method_; @@ -211,7 +222,6 @@ class KeyframeGraphDetector void buildCandidateMatrixSurfTree(); - void buildCorrespondenceMatrix( const KeyframeVector& keyframes, KeyframeAssociationVector& associations); @@ -243,6 +253,8 @@ class KeyframeGraphDetector const KeyframeVector& keyframes); cv::FlannBasedMatcher trainMatcher(const RGBDKeyframe& keyframe); + + }; From ff3bb3cc07e01b1575c989a67f0dd3a3100687ae Mon Sep 17 00:00:00 2001 From: Erik Rosen Date: Mon, 27 Jan 2014 19:45:55 +0000 Subject: [PATCH 07/11] Commit before catkinization --- ccny_rgbd/CMakeLists.txt | 3 + .../include/ccny_rgbd/apps/keyframe_mapper.h | 10 + ccny_rgbd/msg/Keyframe.msg | 2 + ccny_rgbd/src/apps/keyframe_mapper.cpp | 35 +- ccny_rgbd/src/ccny_rgbd/msg/_Keyframe.py | 366 ++++++++++++++++++ 5 files changed, 412 insertions(+), 4 deletions(-) create mode 100644 ccny_rgbd/msg/Keyframe.msg create mode 100644 ccny_rgbd/src/ccny_rgbd/msg/_Keyframe.py diff --git a/ccny_rgbd/CMakeLists.txt b/ccny_rgbd/CMakeLists.txt index b6b935a..2f0afe4 100644 --- a/ccny_rgbd/CMakeLists.txt +++ b/ccny_rgbd/CMakeLists.txt @@ -19,6 +19,9 @@ include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) # Generate services rosbuild_gensrv() +#Generate Messages +rosbuild_genmsg() + # Generate dynamic parameters rosbuild_find_ros_package(dynamic_reconfigure) include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) diff --git a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h index 9aee83a..d496830 100644 --- a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h +++ b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ #include "ccny_rgbd/PublishKeyframes.h" #include "ccny_rgbd/Save.h" #include "ccny_rgbd/Load.h" +#include "ccny_rgbd/Keyframe.h" namespace ccny_rgbd { @@ -206,10 +208,12 @@ class KeyframeMapper private: ros::Publisher keyframes_pub_; ///< ROS publisher for the keyframe point clouds + ros::Publisher pcd_to_octomap_pub_; ///< ROS publisher for keyframe poses and filtered point clouds ros::Publisher poses_pub_; ///< ROS publisher for the keyframe poses ros::Publisher kf_assoc_pub_; ///< ROS publisher for the keyframe associations ros::Publisher path_pub_; ///< ROS publisher for the keyframe path ros::Publisher pose_correction_pub_; ///< ROS publisher for the pose correction transform + /** @brief ROS service to generate the graph correpondences */ ros::ServiceServer generate_graph_service_; @@ -238,6 +242,8 @@ class KeyframeMapper /** @brief ROS service to add a manual keyframe */ ros::ServiceServer add_manual_keyframe_service_; + ros::ServiceClient reset_octomap_client_; + tf::TransformListener tf_listener_; ///< ROS transform listener /** @brief Image transport for RGB message subscription */ @@ -395,6 +401,10 @@ class KeyframeMapper void updatePathFromKeyframePoses(); void publishAggregatedPoseCorrection(); + + void publishKeyframeClouds(void); + + void updateOctoMapServer(void); }; } // namespace ccny_rgbd diff --git a/ccny_rgbd/msg/Keyframe.msg b/ccny_rgbd/msg/Keyframe.msg new file mode 100644 index 0000000..c70ce75 --- /dev/null +++ b/ccny_rgbd/msg/Keyframe.msg @@ -0,0 +1,2 @@ +sensor_msgs/PointCloud2 pcl +geometry_msgs/Pose pose \ No newline at end of file diff --git a/ccny_rgbd/src/apps/keyframe_mapper.cpp b/ccny_rgbd/src/apps/keyframe_mapper.cpp index 8978281..9315afd 100644 --- a/ccny_rgbd/src/apps/keyframe_mapper.cpp +++ b/ccny_rgbd/src/apps/keyframe_mapper.cpp @@ -49,6 +49,12 @@ KeyframeMapper::KeyframeMapper( path_pub_ = nh_.advertise( "mapper_path", queue_size_); pose_correction_pub_ = nh_.advertise("pose_correction_tfs", queue_size_); + + pcd_to_octomap_pub_ = nh_.advertise("keyframes_to_octomap", 100); + + // ***** service clients + + reset_octomap_client_ = nh_.serviceClient("reset"); // **** services @@ -260,6 +266,7 @@ void KeyframeMapper::addKeyframe( keyframe.pose = aggregatedPoseCorrection_ * pose; uncorrected_keyframe_poses_.push_back(pose); int associations_prev = associations_.size(); + keyframe.storeFilteredPointCloud(max_range_,max_stdev_,pcd_map_res_); if (manual_add_) { @@ -276,6 +283,7 @@ void KeyframeMapper::addKeyframe( //Add keyframe to keyframevector keyframes_.push_back(keyframe); + publishKeyframePoses(); //Record the odometry measured between consecutive keyframes (only if more than one keyframe exists) if(keyframes_.size()>1){ @@ -307,6 +315,7 @@ void KeyframeMapper::addKeyframe( publishPath(); publishKeyframePoses(); publishKeyframeAssociations(); + updateOctoMapServer(); //Calculate and publish pose correction AffineTransform poseCorrection; @@ -381,17 +390,24 @@ bool KeyframeMapper::publishKeyframesSrvCallback( return true; } +void KeyframeMapper::publishKeyframeClouds(void){ + for(int i = 0; i < keyframes_.size(); i++){ + publishKeyframeData(i); + } +} + void KeyframeMapper::publishKeyframeData(int i) { rgbdtools::RGBDKeyframe& keyframe = keyframes_[i]; // construct a cloud from the images - PointCloudT cloud; + /*PointCloudT cloud; keyframe.constructDensePointCloud(cloud, max_range_, max_stdev_); + */ // cloud transformed to the fixed frame PointCloudT cloud_ff; - pcl::transformPointCloud(cloud, cloud_ff, keyframe.pose); + pcl::transformPointCloud(keyframe.filteredCloud, cloud_ff, keyframe.pose); cloud_ff.header.frame_id = fixed_frame_; @@ -786,7 +802,7 @@ void KeyframeMapper::buildPcdMap(PointCloudT& map_cloud) for (unsigned int kf_idx = 0; kf_idx < keyframes_.size(); ++kf_idx) { const rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx]; - + /* PointCloudT::Ptr cloud(new PointCloudT()); keyframe.constructDensePointCloud(*cloud, max_range_, max_stdev_); @@ -797,9 +813,10 @@ void KeyframeMapper::buildPcdMap(PointCloudT& map_cloud) sor.setFilterLimits (-std::numeric_limits::infinity(), max_map_z_); PointCloudT cloud_filtered; sor.filter(cloud_filtered); + */ PointCloudT cloud_tf; - pcl::transformPointCloud(cloud_filtered, cloud_tf, keyframe.pose); + pcl::transformPointCloud(keyframe.filteredCloud, cloud_tf, keyframe.pose); cloud_tf.header.frame_id = fixed_frame_; *aggregate_cloud += cloud_tf; @@ -816,6 +833,16 @@ void KeyframeMapper::buildPcdMap(PointCloudT& map_cloud) vgf.filter(map_cloud); } +void KeyframeMapper::updateOctoMapServer(void){ + //Reset map located in octomap server + std_srvs::Empty srv; + reset_octomap_client_.call(srv); + + //Republish all keyframe point clouds + publishKeyframeClouds(); + +} + bool KeyframeMapper::saveOctomap(const std::string& path) { bool result; diff --git a/ccny_rgbd/src/ccny_rgbd/msg/_Keyframe.py b/ccny_rgbd/src/ccny_rgbd/msg/_Keyframe.py new file mode 100644 index 0000000..305dd53 --- /dev/null +++ b/ccny_rgbd/src/ccny_rgbd/msg/_Keyframe.py @@ -0,0 +1,366 @@ +"""autogenerated by genpy from ccny_rgbd/Keyframe.msg. Do not edit.""" +import sys +python3 = True if sys.hexversion > 0x03000000 else False +import genpy +import struct + +import geometry_msgs.msg +import std_msgs.msg +import sensor_msgs.msg + +class Keyframe(genpy.Message): + _md5sum = "982fb77a8a1f36ca5bed86a2ef5998d5" + _type = "ccny_rgbd/Keyframe" + _has_header = False #flag to mark the presence of a Header object + _full_text = """sensor_msgs/PointCloud2 pcl +geometry_msgs/Pose pose +================================================================================ +MSG: sensor_msgs/PointCloud2 +# This message holds a collection of N-dimensional points, which may +# contain additional information such as normals, intensity, etc. The +# point data is stored as a binary blob, its layout described by the +# contents of the "fields" array. + +# The point cloud data may be organized 2d (image-like) or 1d +# (unordered). Point clouds organized as 2d images may be produced by +# camera depth sensors such as stereo or time-of-flight. + +# Time of sensor data acquisition, and the coordinate frame ID (for 3d +# points). +Header header + +# 2D structure of the point cloud. If the cloud is unordered, height is +# 1 and width is the length of the point cloud. +uint32 height +uint32 width + +# Describes the channels and their layout in the binary data blob. +PointField[] fields + +bool is_bigendian # Is this data bigendian? +uint32 point_step # Length of a point in bytes +uint32 row_step # Length of a row in bytes +uint8[] data # Actual point data, size is (row_step*height) + +bool is_dense # True if there are no invalid points + +================================================================================ +MSG: std_msgs/Header +# Standard metadata for higher-level stamped data types. +# This is generally used to communicate timestamped data +# in a particular coordinate frame. +# +# sequence ID: consecutively increasing ID +uint32 seq +#Two-integer timestamp that is expressed as: +# * stamp.secs: seconds (stamp_secs) since epoch +# * stamp.nsecs: nanoseconds since stamp_secs +# time-handling sugar is provided by the client library +time stamp +#Frame this data is associated with +# 0: no frame +# 1: global frame +string frame_id + +================================================================================ +MSG: sensor_msgs/PointField +# This message holds the description of one point entry in the +# PointCloud2 message format. +uint8 INT8 = 1 +uint8 UINT8 = 2 +uint8 INT16 = 3 +uint8 UINT16 = 4 +uint8 INT32 = 5 +uint8 UINT32 = 6 +uint8 FLOAT32 = 7 +uint8 FLOAT64 = 8 + +string name # Name of field +uint32 offset # Offset from start of point struct +uint8 datatype # Datatype enumeration, see above +uint32 count # How many elements in the field + +================================================================================ +MSG: geometry_msgs/Pose +# A representation of pose in free space, composed of postion and orientation. +Point position +Quaternion orientation + +================================================================================ +MSG: geometry_msgs/Point +# This contains the position of a point in free space +float64 x +float64 y +float64 z + +================================================================================ +MSG: geometry_msgs/Quaternion +# This represents an orientation in free space in quaternion form. + +float64 x +float64 y +float64 z +float64 w + +""" + __slots__ = ['pcl','pose'] + _slot_types = ['sensor_msgs/PointCloud2','geometry_msgs/Pose'] + + def __init__(self, *args, **kwds): + """ + Constructor. Any message fields that are implicitly/explicitly + set to None will be assigned a default value. The recommend + use is keyword arguments as this is more robust to future message + changes. You cannot mix in-order arguments and keyword arguments. + + The available fields are: + pcl,pose + + :param args: complete set of field values, in .msg order + :param kwds: use keyword arguments corresponding to message field names + to set specific fields. + """ + if args or kwds: + super(Keyframe, self).__init__(*args, **kwds) + #message fields cannot be None, assign default values for those that are + if self.pcl is None: + self.pcl = sensor_msgs.msg.PointCloud2() + if self.pose is None: + self.pose = geometry_msgs.msg.Pose() + else: + self.pcl = sensor_msgs.msg.PointCloud2() + self.pose = geometry_msgs.msg.Pose() + + def _get_types(self): + """ + internal API method + """ + return self._slot_types + + def serialize(self, buff): + """ + serialize message into buffer + :param buff: buffer, ``StringIO`` + """ + try: + _x = self + buff.write(_struct_3I.pack(_x.pcl.header.seq, _x.pcl.header.stamp.secs, _x.pcl.header.stamp.nsecs)) + _x = self.pcl.header.frame_id + length = len(_x) + if python3 or type(_x) == unicode: + _x = _x.encode('utf-8') + length = len(_x) + buff.write(struct.pack(' Date: Sun, 9 Feb 2014 20:38:42 +0000 Subject: [PATCH 08/11] works with octomap --- .../include/ccny_rgbd/apps/keyframe_mapper.h | 6 +++++ ccny_rgbd/manifest.xml | 1 + ccny_rgbd/src/apps/keyframe_mapper.cpp | 24 ++++++++++++++++--- .../graph/keyframe_graph_solver_g2o.h | 3 +-- .../include/rgbdtools/rgbd_keyframe.h | 4 ++++ 5 files changed, 33 insertions(+), 5 deletions(-) diff --git a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h index d496830..e99096f 100644 --- a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h +++ b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h @@ -41,6 +41,7 @@ #include #include #include +#include #include "ccny_rgbd/types.h" #include "ccny_rgbd/util.h" @@ -319,6 +320,11 @@ class KeyframeMapper * @param i the keyframe index */ void publishKeyframePose(int i); + + /** @brief Publishes the Keyframe point cloud and pose associated with a keyframe + * @param i the keyframe index + */ + void publishKeyframeMsg(int kf_idx); /** @brief Publishes all the keyframe associations markers */ diff --git a/ccny_rgbd/manifest.xml b/ccny_rgbd/manifest.xml index dd328b1..bc2e002 100644 --- a/ccny_rgbd/manifest.xml +++ b/ccny_rgbd/manifest.xml @@ -22,6 +22,7 @@ + diff --git a/ccny_rgbd/src/apps/keyframe_mapper.cpp b/ccny_rgbd/src/apps/keyframe_mapper.cpp index 9315afd..26d6eac 100644 --- a/ccny_rgbd/src/apps/keyframe_mapper.cpp +++ b/ccny_rgbd/src/apps/keyframe_mapper.cpp @@ -50,7 +50,7 @@ KeyframeMapper::KeyframeMapper( "mapper_path", queue_size_); pose_correction_pub_ = nh_.advertise("pose_correction_tfs", queue_size_); - pcd_to_octomap_pub_ = nh_.advertise("keyframes_to_octomap", 100); + pcd_to_octomap_pub_ = nh_.advertise("keyframes_to_octomap", 100); // ***** service clients @@ -282,7 +282,8 @@ void KeyframeMapper::addKeyframe( } //Add keyframe to keyframevector - keyframes_.push_back(keyframe); + keyframes_.push_back(keyframe); + publishKeyframeMsg(keyframes_.size()-1); publishKeyframePoses(); //Record the odometry measured between consecutive keyframes (only if more than one keyframe exists) @@ -392,7 +393,7 @@ bool KeyframeMapper::publishKeyframesSrvCallback( void KeyframeMapper::publishKeyframeClouds(void){ for(int i = 0; i < keyframes_.size(); i++){ - publishKeyframeData(i); + publishKeyframeMsg(i); } } @@ -412,6 +413,7 @@ void KeyframeMapper::publishKeyframeData(int i) cloud_ff.header.frame_id = fixed_frame_; keyframes_pub_.publish(cloud_ff); + } void KeyframeMapper::publishMap(void) @@ -561,6 +563,22 @@ void KeyframeMapper::publishKeyframePose(int i) poses_pub_.publish(marker_text); } +void KeyframeMapper::publishKeyframeMsg(int kf_idx){ + rgbdtools::RGBDKeyframe& keyframe = keyframes_[kf_idx]; + + qbo_graph_slam_messages::Keyframe kf_msg; + tf::Transform keyframe_pose = tfFromEigenAffine(keyframe.pose); + tf::poseTFToMsg(keyframe_pose,kf_msg.pose); + + PointCloudT cloud_ff; + pcl::transformPointCloud(keyframe.filteredCloud, cloud_ff, keyframe.pose); + + cloud_ff.header.frame_id = fixed_frame_; + + pcl::toROSMsg ( cloud_ff,kf_msg.pcl); + pcd_to_octomap_pub_.publish(kf_msg); +} + bool KeyframeMapper::saveKeyframesSrvCallback( Save::Request& request, Save::Response& response) diff --git a/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver_g2o.h b/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver_g2o.h index 2b7e0d7..f4ddcdf 100644 --- a/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver_g2o.h +++ b/lib_rgbdtools/include/rgbdtools/graph/keyframe_graph_solver_g2o.h @@ -53,8 +53,7 @@ class KeyframeGraphSolverG2O: public KeyframeGraphSolver * @param keyframes vector of keyframes * @param associations vector of input keyframe associations */ - void solve(KeyframeVector& keyframes, const KeyframeAssociationVector & associations, const KeyframeAssociationVector & odometryEdges); - + void solve(KeyframeVector& keyframes, const KeyframeAssociationVector & associations, const KeyframeAssociationVector & odometryEdges); void solve(KeyframeVector& keyframes, const KeyframeAssociationVector& associations, diff --git a/lib_rgbdtools/include/rgbdtools/rgbd_keyframe.h b/lib_rgbdtools/include/rgbdtools/rgbd_keyframe.h index 83ce463..5e26855 100644 --- a/lib_rgbdtools/include/rgbdtools/rgbd_keyframe.h +++ b/lib_rgbdtools/include/rgbdtools/rgbd_keyframe.h @@ -90,6 +90,10 @@ class RGBDKeyframe: public RGBDFrame */ static bool load(RGBDKeyframe& keyframe, const std::string& path); + + void storeFilteredPointCloud(double max_range, double max_stdev,double leaf_size); + + PointCloudT filteredCloud; }; typedef Eigen::aligned_allocator KeyframeAllocator; From dfdeccabcb1e2c205da120dccd8df2d0eb4514c2 Mon Sep 17 00:00:00 2001 From: Erik Rosen Date: Sun, 9 Feb 2014 22:38:36 +0000 Subject: [PATCH 09/11] Removed Keyframe message --- ccny_rgbd/msg/Keyframe.msg | 2 -- 1 file changed, 2 deletions(-) delete mode 100644 ccny_rgbd/msg/Keyframe.msg diff --git a/ccny_rgbd/msg/Keyframe.msg b/ccny_rgbd/msg/Keyframe.msg deleted file mode 100644 index c70ce75..0000000 --- a/ccny_rgbd/msg/Keyframe.msg +++ /dev/null @@ -1,2 +0,0 @@ -sensor_msgs/PointCloud2 pcl -geometry_msgs/Pose pose \ No newline at end of file From a4cd6816da929034ed8212cf7649fd7bb3b07958 Mon Sep 17 00:00:00 2001 From: Erik Rosen Date: Sun, 9 Feb 2014 22:58:52 +0000 Subject: [PATCH 10/11] Removed keyframe header file requirement --- ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h | 1 - 1 file changed, 1 deletion(-) diff --git a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h index e99096f..6ad07a0 100644 --- a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h +++ b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h @@ -52,7 +52,6 @@ #include "ccny_rgbd/PublishKeyframes.h" #include "ccny_rgbd/Save.h" #include "ccny_rgbd/Load.h" -#include "ccny_rgbd/Keyframe.h" namespace ccny_rgbd { From 29f58b8bdaa30c4231c269a604f7574a557d113d Mon Sep 17 00:00:00 2001 From: Erik Rosen Date: Tue, 11 Feb 2014 16:10:39 +0000 Subject: [PATCH 11/11] Added max ang vel as a parameter passed into the mapping node --- ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h | 1 + ccny_rgbd/src/apps/keyframe_mapper.cpp | 11 ++++++----- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h index 6ad07a0..91804b1 100644 --- a/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h +++ b/ccny_rgbd/include/ccny_rgbd/apps/keyframe_mapper.h @@ -269,6 +269,7 @@ class KeyframeMapper void updateOdom(const nav_msgs::Odometry & odom_msg); // params + double max_ang_vel_; ///< maximum angular velocity of robot for keyframe to be added double pcd_map_res_; ///< downsampling resolution of pcd map (in meters) double octomap_res_; ///< tree resolution for octomap (in meters) double kf_dist_eps_; ///< linear distance threshold between keyframes diff --git a/ccny_rgbd/src/apps/keyframe_mapper.cpp b/ccny_rgbd/src/apps/keyframe_mapper.cpp index 26d6eac..3a2cc14 100644 --- a/ccny_rgbd/src/apps/keyframe_mapper.cpp +++ b/ccny_rgbd/src/apps/keyframe_mapper.cpp @@ -128,7 +128,8 @@ void KeyframeMapper::initParams() max_map_z_ = std::numeric_limits::infinity(); if (!nh_private_.getParam ("online_graph_opt", online_graph_opt_)) online_graph_opt_ = false; - + if (!nh_private_.getParam ("max_ang_vel", max_ang_vel_)) + max_ang_vel_ = 9999; // configure graph detection @@ -232,7 +233,7 @@ bool KeyframeMapper::processFrame( getTfDifference(tfFromEigenAffine(aggregatedPoseCorrection_ *pose), tfFromEigenAffine(keyframes_.back().pose), dist, angle); - + ROS_INFO("Distance travelled & degrees turned since last KF: %f m, %f degrees",dist,angle); if (dist > kf_dist_eps_ || angle > kf_angle_eps_){ result = true; } @@ -241,12 +242,12 @@ bool KeyframeMapper::processFrame( } } - if(fabs(angularVelocity_)>0.2){ + if(fabs(angularVelocity_)>max_ang_vel_){ result = false; - printf("Angular velocity too large! Will not add keyframe \n"); + ROS_INFO("Angular velocity too large! Will not add keyframe"); } else{ - printf("Angular velocity: %f \n", angularVelocity_); + //printf("Angular velocity: %f \n", angularVelocity_); }