diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..acd8878 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,36 @@ +sudo: required +dist: trusty +language: generic +compiler: + - gcc +notifications: + email: + on_success: always + on_failure: always +# recipients: +# - jane@doe +env: + matrix: + - USE_DEB=true ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu + - USE_DEB=true ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu + - USE_DEB=true ROS_DISTRO="indigo" PRERELEASE=true + - USE_DEB=true ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu + - USE_DEB=true ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu + - USE_DEB=true ROS_DISTRO="jade" PRERELEASE=true + - USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu + - USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu + - USE_DEB=true ROS_DISTRO="kinetic" PRERELEASE=true +matrix: + allow_failures: + - env: USE_DEB=true ROS_DISTRO="indigo" PRERELEASE=true + - env: USE_DEB=true ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu + - env: USE_DEB=true ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu + - env: USE_DEB=true ROS_DISTRO="jade" PRERELEASE=true + - env: USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu + - env: USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu + - env: USE_DEB=true ROS_DISTRO="kinetic" PRERELEASE=true +install: + - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config +script: + - source .ci_config/travis.sh +# - source ./travis.sh # Enable this when you have a package-local script diff --git a/CMakeLists.txt b/CMakeLists.txt index e63f966..a9a1172 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,19 +3,28 @@ project(ratslam_ros) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake) +# Enable Cxx11 support +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) +if(COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "-std=c++11") +elseif(COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "-std=c++0x") +else() + message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") +endif() ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS message_generation std_msgs roscpp sensor_msgs nav_msgs tf visualization_msgs image_transport nav_msgs) ## actionlib_msgs) -find_package(OpenCV) -include_directories(${OpenCV_INCLUDE_DIRS}) +find_package(OpenCV REQUIRED) find_package(Boost REQUIRED COMPONENTS serialization) find_package(Irrlicht REQUIRED) find_package(OpenGL REQUIRED) - ## Generate messages in the 'msg' folder add_message_files( FILES @@ -58,45 +67,53 @@ CATKIN_DEPENDS message_runtime geometry_msgs std_msgs ## Your package locations should be listed before other locations # include_directories(include) include_directories( + src + ${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) #ratslam library add_library(ratslam src/ratslam/experience_map.cpp src/ratslam/posecell_network.cpp src/ratslam/local_view_match.cpp src/ratslam/visual_odometry.cpp) +target_link_libraries(ratslam + ${Boost_LIBRARIES} + ${OpenCV_LIBRARIES} +) -# uncomment is you don't have irrlicht installed +# comment this out, if you don't have irrlicht installed add_definitions("-DHAVE_IRRLICHT") add_executable(ratslam_lv src/main_lv.cpp) -target_link_libraries(ratslam_lv - ${catkin_LIBRARIES} +target_link_libraries(ratslam_lv ratslam ${IRRLICHT_LIBRARIES} ${OPENGL_LIBRARIES} - ${OpenCV_LIBRARIES}) + ${catkin_LIBRARIES} +) add_executable(ratslam_pc src/main_pc.cpp) target_link_libraries(ratslam_pc - ${catkin_LIBRARIES} ratslam ${IRRLICHT_LIBRARIES} - ${OPENGL_LIBRARIES}) + ${OPENGL_LIBRARIES} + ${catkin_LIBRARIES} +) add_executable(ratslam_em src/main_em.cpp) target_link_libraries(ratslam_em - ${catkin_LIBRARIES} ratslam ${IRRLICHT_LIBRARIES} - ${OPENGL_LIBRARIES}) + ${OPENGL_LIBRARIES} + ${catkin_LIBRARIES} +) add_executable(ratslam_vo src/main_vo.cpp) target_link_libraries(ratslam_vo - ${catkin_LIBRARIES} ratslam ${IRRLICHT_LIBRARIES} ${OPENGL_LIBRARIES} - ${OpenCV_LIBRARIES}) + ${catkin_LIBRARIES} +) #config files for devel set(MEDIA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/src/media) #devel use the files in the source dir @@ -145,3 +162,22 @@ install(DIRECTORY src/media/ install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) + +if (CATKIN_ENABLE_TESTING) + +## setup test data download target +add_custom_target( + irat_aus_test.bag + COMMAND if [ ! -e ./tests ]\; then mkdir tests\; fi + COMMAND if [ ! -e ./tests/irat_aus_test.bag ]\; then wget https://www.dropbox.com/s/ukiob6h0reavhem/irat_aus_test.bag?dl=0 -O tests/irat_aus_test.bag\; fi + WORKING_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +## add download target to test target dependencies +add_dependencies(tests irat_aus_test.bag) + +## add rostests +find_package(rostest REQUIRED) +add_rostest(tests/irat_aus.test) + +endif() diff --git a/README.md b/README.md new file mode 100644 index 0000000..3ddf1dc --- /dev/null +++ b/README.md @@ -0,0 +1,173 @@ +# RatSLAM ROS (OpenRatSLAM) + +[![Build Status](https://travis-ci.org/sem23/ratslam.svg?branch=ratslam_ros)](https://travis-ci.org/sem23/ratslam) + +## Package Summary + +This package contains the RatSLAM sources including ROS wrappers. The package provides a biological inspired monocular +vision SLAM (Simultaneous Localization and Mapping) system. Using this system you can create a topological map of the +environment, which means there are just paths and **no** gridmap, like an occupancy grid. As a gimmick, the system is +able to perfom pathplanning on the current map. + +Authors Documentation:
+The ROS version of RatSLAM has been designed as a more modular version than the other library in this project - the algorithm has been divided into 3 individual RatSLAM ROS nodes and one visual odometry node. This version is referred to as OpenRatSLAM. + +OpenRatSLAM is based on the RatSLAM C++ library written by David Ball and Scott Heath, with support and additions by Michael Milford. The RatSLAM algorithm was originally designed and implemented by Michael Milford and Gordon Wyeth. + +OpenRatSLAM has only been tested on Ubuntu and as ROS support for platforms other than Linux is still limited, this will probably not change. + +* Maintainer status: maintained +* Maintainers: David M. Ball \, Peter Rudolph \ +* Authors: Michael Milford and Gordon Wyeth (RatSLAM), David Ball and Scott Heath (RatSLAM C++), David M. Ball (OpenRatSLAM) +* License: GNU General Public License v3.0 +* Source: git https://github.com/sem23/ratslam.git (branch: ratslam_ros) + +## 1. External Documentation + +[David Ball, Scott Heath, Janet Wiles, Gordon Wyeth, Peter Corke, Michael Milford: OpenRatSLAM: an open source brain-based SLAM system, Autonomous Robots, 2013](https://link.springer.com/article/10.1007%2Fs10514-012-9317-9). + +## 2. Hardware Requirements + +To get RatSLAM working you need a properly setup monocular camera and an odometry source. The odometry can be generated +using the visual odometry node, which is provided by the package. But note, that a wheel based odometry is much more +accurate in most cases. + +## 3. Example + +Several examples are provided [here](https://github.com/davidmball/ratslam/blob/wiki/RatSLAMROS.md#running-ratslam). + +## 4. Nodes + +### 4.1 ratslam_em + +The experience map is used to store the experiences made by the robot. This node provides a topological map, loop closing capabilites +and a dijkstra pathplanning algorithm. + +#### 4.1.1 Subscribed Topics + +"odom" ([nav_msgs/Odometry](http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html)) +> The current odometry. + +topic_root + "/PoseCell/TopologicalAction" (ratslam_ros/TopologicalAction) +> The topological action. + +topic_root + "/ExperienceMap/SetGoalPose" ([geometry_msgs/PoseStamped](http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html)) +> The goal pose to where path will be planned. + +#### 4.1.2 Published Topics + +topic_root + "/ExperienceMap/Map" (ratslam_ros/TopologicalMap) +> The experience map in RatSLAM format. + +topic_root + "/ExperienceMap/MapMarker" ([visualization_msgs/Marker](http://docs.ros.org/api/visualization_msgs/html/msg/Marker.html)) +> The experience map as ROS visualization markers for displaying puposes. + +topic_root + "/ExperienceMap/RobotPose" ([geometry_msgs/PoseStamped](http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html)) +> The current pose of the robot in map. + +topic_root + "/ExperienceMap/PathToGoal" ([nav_msgs/Path](http://docs.ros.org/api/nav_msgs/html/msg/Path.html)) +> The current planned path (published if triggered to plan). + +#### 4.1.3 Parameters + +**Note:** Only ROS parameters are mentioned. All others are explained in external documentation. The parameter "topic_root" can be found there! + +"map_frame" (Default: "map") +> The name of the map frame. + +"odom_frame" (Default: "odom") +> The name of the odometry frame. + +"base_frame" (Default: "base_link") +> The name of the base frame. + +"tf_update_rate" (Default: 20.0) +> The update rate [hz] of the map_frame -> odom_frame transformation broadcaster. + +"map_save_period" (Default: 10.0) +> The period [seconds] after which a map is saved. + +"map_file_path" (Default: "ratslam-latest.bmap") +> The file path where map is saved. + +#### 4.1.4 Required tf Transforms + +odom_frame -> base_frame +> Usually provided by the odometry system (e.g., the driver for the mobile base). + +#### 4.1.5 Provided tf Transforms + +map_frame -> odom_frame +> The current estimate of the robot's pose within the map frame. + +### 4.2 ratslam_pc + +The pose cell network is used to track odometry information based on movement, vision and recognition. + +#### 4.2.1 Subscribed Topics + +"odom" ([nav_msgs/Odometry](http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html)) +> The current odometry. + +topic_root + "/LocalView/Template" (ratslam_ros/ViewTemplate) +> The current matched template. + +#### 4.2.2 Published Topics + +topic_root + "/PoseCell/TopologicalAction" (ratslam_ros/TopologicalMap) +> The topological action performed by network. + +#### 4.2.3 Parameters + +**Note:** Only ROS parameters are mentioned. All others are explained in external documentation. + +"pcn_save_period" (Default: 10.0) +> The period [seconds] after which a pose cell network state is saved. + +"pcn_file_path" (Default: "ratslam-latest.bpcn") +> The file path where pose cell network state is saved. + +### 4.3 ratslam_lv + +The local view matcher is used to match the camera templates against previous experienced ones. This node provides the matching of images. + +#### 4.3.1 Subscribed Topics + +"image" ([sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html)) +> ImageTransport image subscriber. + +"camera_info" ([sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html)) +> ImageTransport camera info subscriber. + +#### 4.3.2 Published Topics + +topic_root + "/LocalView/Template" (ratslam_ros/ViewTemplate) +> The current matched template. + +#### 4.3.3 Parameters + +**Note:** Only ROS parameters are mentioned. All others are explained in external documentation. The parameter "topic_root" can be found there! + +"lvm_save_period" (Default: 10.0) +> The period [seconds] after which a local view matcher state is saved. + +"lvm_file_path" (Default: "ratslam-latest.blvm") +> The file path where local view matcher state is saved. + +### 4.4 ratslam_vo + +The visual odometry node. This node can be used to generate an odometry source from monocular camera input. +**Note:** Using wheel based odometry is more accurate in most cases. + +#### 4.4.1 Subscribed Topics + +"image" ([sensor_msgs/Image](http://docs.ros.org/api/sensor_msgs/html/msg/Image.html)) +> ImageTransport image subscriber. + +"camera_info" ([sensor_msgs/CameraInfo](http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html)) +> ImageTransport camera info subscriber. + +#### 4.4.2 Published Topics + +"odom" ([nav_msgs/Odometry](http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html)) +> The current odometry generated from vision source. diff --git a/config/config_irataus_test.txt.in b/config/config_irataus_test.txt.in new file mode 100644 index 0000000..9ff5449 --- /dev/null +++ b/config/config_irataus_test.txt.in @@ -0,0 +1,28 @@ +[general] +topic_root=irat_red + +[ratslam] +image_crop_y_max=150 +template_x_size=60 +template_y_size=20 +vt_shift_match = 4 +vt_step_match = 1 +vt_match_threshold = 0.03 +vt_active_decay = 1.0 + +pc_dim_xy = 11 +pc_vt_inject_energy = 0.1 +pc_cell_x_size = 0.015 + +exp_delta_pc_threshold = 2 +exp_loops=20 +exp_initial_em_deg=140 + +[draw] +enable=0 +vt_window_width=416 +vt_window_height=480 +exp_map_size=500 +posecells_size=250 +media_path=@MEDIA_PATH@ +image_file=irat_sm.tga diff --git a/package.xml b/package.xml index b9434fe..305b5bb 100644 --- a/package.xml +++ b/package.xml @@ -1,43 +1,56 @@ ratslam_ros - 0.0.1 + 1.0.0 ratslam_ros - scott + Michael Milford + Gordon Wyeth + David Ball + Scott Heath + David Ball + Peter Rudolph BSD http://ros.org/wiki/ratslam catkin + libirrlicht-dev + opengl + cv_bridge std_msgs geometry_msgs roscpp sensor_msgs nav_msgs - opencv2 tf visualization_msgs image_transport + compressed_image_transport nav_msgs boost message_generation message_runtime + wget + libirrlicht-dev + opengl + cv_bridge std_msgs geometry_msgs roscpp sensor_msgs nav_msgs - opencv2 tf visualization_msgs image_transport + compressed_image_transport nav_msgs boost message_generation message_runtime + wget diff --git a/script/path_follower.py b/script/path_follower.py new file mode 100755 index 0000000..c3cd2fc --- /dev/null +++ b/script/path_follower.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python +# -*- encoding: utf-8 -*- +""" +This file implements a naive path following controller. + +Note: As the controller does not care about collision + avoidance so the path must be carefully planned + and so to say "drivable"! Changing environment + and moving object will be a game-killer. + +Copyright {2017} {Peter Rudolph} + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. + +""" + +# ROS +import numpy as np +import rospy +from tf import ExtrapolationException, LookupException +from tf.listener import TransformListener + +# ROS msgs +from geometry_msgs.msg import Twist +from nav_msgs.msg import Path + + +class PathFollowController(object): + """ + This implements a naive path following controller. + + """ + + def __init__(self): + # vars + self._accept_path = True + self._path = Path() + + # params + self._update_rate = rospy.get_param('~update_rate', 10) + self._base_frame = rospy.get_param('~base_frame', 'base_footprint') + self._min_lin_vel = rospy.get_param('~min_lin_vel', 0.2) + self._max_lin_vel = rospy.get_param('~max_lin_vel', 1.0) + self._min_ang_vel = rospy.get_param('~min_ang_vel', 15. * np.pi / 180.) + self._max_ang_vel = rospy.get_param('~max_ang_vel', 45. * np.pi / 180.) + self._min_goal_dist = rospy.get_param('~min_goal_dist', 0.05) + self._min_drive_angle = rospy.get_param('~min_drive_angle', 25.0 * np.pi / 180.) + + # tf + self._tfl = TransformListener() + + # pubs + self._cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) + + # subs + self._path_sub = rospy.Subscriber('path', Path, self._path_cb, queue_size=1) + + def _path_cb(self, msg): + """ + The path callback updating the current path. + + :type msg: Path + :param msg: The path message. + + """ + if self._accept_path: + # setup current path + self._path = msg + # do not accept new paths + self._accept_path = False + # log + rospy.loginfo('Received new path!') + + def run(self, update_rate=20): + """ + Continuously drives to goals in current path. + + If goal is reached it is removed from current path. + + :type update_rate: int + :param update_rate: The loop update rate. + + """ + r = rospy.Rate(update_rate) + while not rospy.is_shutdown(): + + # do nothing else than accepting new paths + # if we do not have a current path + if len(self._path.poses) == 0: + # accept new paths + self._accept_path = True + r.sleep() + continue + + # get current goal + goal_pose_ = self._path.poses[-1] + + # try transform goal to base frame + try: + goal_pose = self._tfl.transformPose(self._base_frame, goal_pose_) + except (ExtrapolationException, LookupException): + rospy.logwarn('Could not get transform {} -> {}.'.format(goal_pose_.header.frame_id, self._base_frame)) + r.sleep() + continue + + # distance to goal pose + x = goal_pose.pose.position.x + y = goal_pose.pose.position.y + dist = np.sqrt(x**2+y**2) + angle = np.math.atan2(y, x) + + # remove goal if we are close too + # TODO: turn towards last goals heading + if dist < self._min_goal_dist: + self._path.poses.pop() + + # setup commanded velocities + cmd_vel = Twist() + cmd_vel.angular.z = angle + + # limit turn rate (angular velocity) + if abs(angle) > self._max_ang_vel: + cmd_vel.angular.z = max(min(dist, self._max_ang_vel), self._min_ang_vel) * np.sign(cmd_vel.angular.z) + + # only drive if we are almost facing the goal + if abs(angle) < self._min_drive_angle: + # limit speed to distance or max speed + cmd_vel.linear.x = max(min(dist, self._max_lin_vel), self._min_lin_vel) + + # publish velocities + self._cmd_vel_pub.publish(cmd_vel) + + # sleep + r.sleep() + +# initialize ROS node +rospy.init_node('naive_path_follower') +# initialize controller +pfc = PathFollowController() +# loop +pfc.run() diff --git a/src/graphics/experience_map_scene.h b/src/graphics/experience_map_scene.h index afd2011..b62f274 100644 --- a/src/graphics/experience_map_scene.h +++ b/src/graphics/experience_map_scene.h @@ -35,20 +35,27 @@ #include #define _USE_MATH_DEFINES + #include #include "path_node.h" namespace ratslam { - class ExperienceMapScene { public: - ExperienceMapScene(ptree & settings, ExperienceMap *in_map) : - exp_map_scene(NULL), exp_map_path(NULL), exp_map_goal_path(NULL), exp_map_exps(NULL), map(in_map) + ExperienceMapScene(ptree& settings, ExperienceMap* in_map) + : exp_map_scene(NULL) + , exp_map_path(NULL) + , exp_map_goal_path(NULL) + , exp_map_exps(NULL) + , max_x(DBL_MAX) + , min_x(-DBL_MAX) + , max_y(DBL_MAX) + , min_y(-DBL_MAX) + , map(in_map) { - int width = 800; int height = 800; @@ -59,9 +66,9 @@ class ExperienceMapScene driver = device->getVideoDriver(); scene = device->getSceneManager(); - irr::scene::ICameraSceneNode * camera = scene->addCameraSceneNode( - NULL, irr::core::vector3df((irr::f32)(0.5 * width), (irr::f32)(0.5 * height), -10), - irr::core::vector3df((irr::f32)(0.5 * width), (irr::f32)(0.5 * height), 0)); + irr::scene::ICameraSceneNode* camera = + scene->addCameraSceneNode(NULL, irr::core::vector3df((irr::f32)(0.5 * width), (irr::f32)(0.5 * height), -10), + irr::core::vector3df((irr::f32)(0.5 * width), (irr::f32)(0.5 * height), 0)); irr::core::matrix4 proj; proj.buildProjectionMatrixOrthoLH((irr::f32)width, (irr::f32)height, (irr::f32)0.01, (irr::f32)100); camera->setProjectionMatrix(proj, true); @@ -81,12 +88,12 @@ class ExperienceMapScene exp_map_goal_path->getMaterial(0).Thickness = 2.0f; // todo: use relative path here and if can't find the image then draw a simple robot shape box - get_setting_from_ptree(media_path, settings, "media_path", (std::string)""); - get_setting_from_ptree(image_file, settings, "image_file", (std::string)""); + get_setting_from_ptree(media_path, settings, "media_path", (std::string) ""); + get_setting_from_ptree(image_file, settings, "image_file", (std::string) ""); // add the irat texture - irr::video::ITexture * irat_texture = exp_map_scene->getVideoDriver()->getTexture( - (media_path + "/" + image_file).c_str()); + irr::video::ITexture* irat_texture = + exp_map_scene->getVideoDriver()->getTexture((media_path + "/" + image_file).c_str()); if (irat_texture == 0) { std::cout << "WARNING: Unable to load texture: " << (media_path + "/" + image_file) << std::endl; @@ -95,29 +102,27 @@ class ExperienceMapScene // add a cube node and texture it with the irat if (irat_texture) { - irat_node = exp_map_scene->addMeshSceneNode( - exp_map_scene->getGeometryCreator()->createCubeMesh( - irr::core::vector3df((irr::f32)(irat_texture->getSize().Width / 8.0), - (irr::f32)(irat_texture->getSize().Height / 8.0), (irr::f32)0.1)), - NULL); + irat_node = + exp_map_scene->addMeshSceneNode(exp_map_scene->getGeometryCreator()->createCubeMesh(irr::core::vector3df( + (irr::f32)(irat_texture->getSize().Width / 8.0), + (irr::f32)(irat_texture->getSize().Height / 8.0), (irr::f32)0.1)), + NULL); } else { - irat_node = exp_map_scene->addMeshSceneNode( - exp_map_scene->getGeometryCreator()->createCubeMesh( - irr::core::vector3df((irr::f32)(535.0 / 8.0), - (irr::f32)(289.0 / 8.0), (irr::f32)0.1)), - NULL); + irat_node = + exp_map_scene->addMeshSceneNode(exp_map_scene->getGeometryCreator()->createCubeMesh(irr::core::vector3df( + (irr::f32)(535.0 / 8.0), (irr::f32)(289.0 / 8.0), (irr::f32)0.1)), + NULL); } irat_node->getMaterial(0).EmissiveColor = irr::video::SColor(255, 255, 255, 255); - if (irat_texture) - { - irat_node->getMaterial(0).setTexture(0, irat_texture); - } + if (irat_texture) + { + irat_node->getMaterial(0).setTexture(0, irat_texture); + } irat_node->getMaterial(0).MaterialType = irr::video::EMT_TRANSPARENT_ALPHA_CHANNEL_REF; update_viewport(0, 0, width, height); - } ~ExperienceMapScene() @@ -130,7 +135,7 @@ class ExperienceMapScene void update_experiences() { - double x, y; //, facing; + double x, y; //, facing; unsigned int i; min_x = DBL_MAX; @@ -162,7 +167,6 @@ class ExperienceMapScene { max_y = y; } - } // account for the size of the robot @@ -170,7 +174,6 @@ class ExperienceMapScene min_y = min_y - 0.1; max_x = max_x + 0.1; max_y = max_y + 0.1; - } void update_links() @@ -194,11 +197,11 @@ class ExperienceMapScene y2d1 = (int)(((map->experiences[exp1_id].y_m - min_y) / (max_y - min_y) * viewport_height)); x2d2 = (int)(((map->experiences[exp2_id].x_m - min_x) / (max_x - min_x) * viewport_width)); y2d2 = (int)(((map->experiences[exp2_id].y_m - min_y) / (max_y - min_y) * viewport_height)); - exp_map_goal_path->addPoint(irr::core::vector3df((irr::f32)x2d1, (irr::f32)y2d1, (irr::f32)-1.0)); - exp_map_goal_path->addPoint(irr::core::vector3df((irr::f32)x2d2, (irr::f32)y2d2, (irr::f32)-1.0)); + exp_map_goal_path->addPoint(irr::core::vector3df((irr::f32)x2d1, (irr::f32)y2d1, (irr::f32) - 1.0)); + exp_map_goal_path->addPoint(irr::core::vector3df((irr::f32)x2d2, (irr::f32)y2d2, (irr::f32) - 1.0)); } - void update_goal_list(const std::deque &goals) + void update_goal_list(const std::deque& goals) { unsigned int i; @@ -213,10 +216,7 @@ class ExperienceMapScene wchar_t buffer[200]; swprintf(buffer, 200, L"%i", i); numberNodes[i] = exp_map_scene->addBillboardTextSceneNode( - exp_map_scene->getGUIEnvironment()->getBuiltInFont(), - buffer, - NULL, - irr::core::dimension2d(50, 50), + exp_map_scene->getGUIEnvironment()->getBuiltInFont(), buffer, NULL, irr::core::dimension2d(50, 50), irr::core::vector3df((irr::f32)((map->experiences[goals[i]].x_m - min_x) / (max_x - min_x) * viewport_width), (irr::f32)((map->experiences[goals[i]].y_m - min_y) / (max_y - min_y) * viewport_height), (irr::f32)0.0), @@ -228,10 +228,7 @@ class ExperienceMapScene wchar_t buffer[200]; swprintf(buffer, 200, L"x"); numberNodes[i] = exp_map_scene->addBillboardTextSceneNode( - exp_map_scene->getGUIEnvironment()->getBuiltInFont(), - buffer, - NULL, - irr::core::dimension2d(50, 50), + exp_map_scene->getGUIEnvironment()->getBuiltInFont(), buffer, NULL, irr::core::dimension2d(50, 50), irr::core::vector3df( (irr::f32)((map->experiences[map->waypoint_exp_id].x_m - min_x) / (max_x - min_x) * viewport_width), (irr::f32)((map->experiences[map->waypoint_exp_id].y_m - min_y) / (max_y - min_y) * viewport_height), @@ -258,7 +255,7 @@ class ExperienceMapScene map->experiences[exp].th_rad += 2 * M_PI; } - double agent_rad = map->experiences[exp].th_rad + map->relative_rad; + double agent_rad = map->experiences[exp].th_rad + map->relative_rad; double facing_ratio = tan(agent_rad); if (agent_rad > 0 && agent_rad < M_PI_2) @@ -291,8 +288,9 @@ class ExperienceMapScene viewport_height = height; // setup an orthogonal matrix - irr::scene::ICameraSceneNode * exp_map_camera = exp_map_scene->addCameraSceneNode( - NULL, irr::core::vector3df((irr::f32)(0.5 * viewport_width), (irr::f32)(0.5 * viewport_height), (irr::f32)-40), + irr::scene::ICameraSceneNode* exp_map_camera = exp_map_scene->addCameraSceneNode( + NULL, + irr::core::vector3df((irr::f32)(0.5 * viewport_width), (irr::f32)(0.5 * viewport_height), (irr::f32) - 40), irr::core::vector3df((irr::f32)(0.5 * viewport_width), (irr::f32)(0.5 * viewport_height), (irr::f32)0)); irr::core::matrix4 proj; proj.buildProjectionMatrixOrthoLH((irr::f32)(1.0 * viewport_width), (irr::f32)(1.0 * viewport_height), @@ -302,9 +300,9 @@ class ExperienceMapScene void set_viewport() { - exp_map_scene->getVideoDriver()->setViewPort( - irr::core::rect((irr::s32)viewport_x, (irr::s32)viewport_y, (irr::s32)(viewport_x + viewport_width), - (irr::s32)(viewport_y + viewport_height))); + exp_map_scene->getVideoDriver()->setViewPort(irr::core::rect((irr::s32)viewport_x, (irr::s32)viewport_y, + (irr::s32)(viewport_x + viewport_width), + (irr::s32)(viewport_y + viewport_height))); } void update_scene() @@ -329,7 +327,6 @@ class ExperienceMapScene } update_goal_list(map->goal_list); - } void draw_all() @@ -340,29 +337,29 @@ class ExperienceMapScene driver->endScene(); } - void screen_to_world(int x, int y, double & x_m, double & y_m) + void screen_to_world(int x, int y, double& x_m, double& y_m) { x_m = (double)(x - viewport_x) / viewport_width * (max_x - min_x) + min_x; y_m = (double)max_y - (y - viewport_y) / viewport_height * (max_y - min_y); } - void update_ptr(ExperienceMap *in_map) + void update_ptr(ExperienceMap* in_map) { map = in_map; } private: - irr::scene::ISceneManager * exp_map_scene; - PathNode * exp_map_path; - PathNode * exp_map_exps; - PathNode * exp_map_goal_path; - irr::scene::IMeshSceneNode * irat_node; + irr::scene::ISceneManager* exp_map_scene; + PathNode* exp_map_path; + PathNode* exp_map_exps; + PathNode* exp_map_goal_path; + irr::scene::IMeshSceneNode* irat_node; std::string media_path; std::string image_file; - irr::IrrlichtDevice *device; - irr::video::IVideoDriver * driver; - irr::scene::ISceneManager * scene; + irr::IrrlichtDevice* device; + irr::video::IVideoDriver* driver; + irr::scene::ISceneManager* scene; double min_x; double max_x; @@ -373,12 +370,9 @@ class ExperienceMapScene std::vector numberNodes; - ratslam::ExperienceMap *map; + ratslam::ExperienceMap* map; +}; }; - -} -; // namespace ratslam #endif - diff --git a/src/graphics/local_view_scene.h b/src/graphics/local_view_scene.h index 8762ae7..a966c92 100644 --- a/src/graphics/local_view_scene.h +++ b/src/graphics/local_view_scene.h @@ -37,72 +37,66 @@ namespace ratslam { - class LocalViewScene { public: - LocalViewScene(ptree & settings, LocalViewMatch *in_vt) + LocalViewScene(ptree& settings, LocalViewMatch* in_vt) { - get_setting_from_ptree(vt_window_width, settings, "vt_window_width", 640); get_setting_from_ptree(vt_window_height, settings, "vt_window_height", 480); update_ptr(in_vt); // the camera image is in the top half and the two template windows in the bottom half - // vt_window_height = vtm->IMAGE_HEIGHT * ((double)vt_window_width/vtm->IMAGE_WIDTH) * 2; + // vt_window_height = vtm->IMAGE_HEIGHT * ((double)vt_window_width/vtm->IMAGE_WIDTH) * 2; - device = irr::createDevice(irr::video::EDT_OPENGL, irr::core::dimension2d(vt_window_width, vt_window_height), 32, false, - false, false); + device = + irr::createDevice(irr::video::EDT_OPENGL, irr::core::dimension2d(vt_window_width, vt_window_height), + 32, false, false, false); device->setWindowCaption(L"openRatSLAM Local View"); driver = device->getVideoDriver(); scene = device->getSceneManager(); - view_template_scene = scene->createNewSceneManager(false); - } ~LocalViewScene() { - } void draw_all() { - device->run(); // TODO return the bool for quiting + device->run(); // TODO return the bool for quiting driver->beginScene(true, true, irr::video::SColor(255, 0, 0, 0)); // TODO not always true for greyscale - draw_image(vtm->view_rgb, vtm->greyscale, -1.0f, 1.0f, vtm->IMAGE_WIDTH, vtm->IMAGE_HEIGHT, (double)vt_window_width/vtm->IMAGE_WIDTH, -(double)vt_window_width/vtm->IMAGE_WIDTH); + draw_image(vtm->view_rgb, vtm->greyscale, -1.0f, 1.0f, vtm->IMAGE_WIDTH, vtm->IMAGE_HEIGHT, + (double)vt_window_width / vtm->IMAGE_WIDTH, -(double)vt_window_width / vtm->IMAGE_WIDTH); - draw_image((const double*)&vtm->templates[vtm->current_vt].data[0], true, -1, 0.0, - vtm->TEMPLATE_X_SIZE, vtm->TEMPLATE_Y_SIZE, - (double)vt_window_width/vtm->TEMPLATE_X_SIZE, - -(double)vt_window_height/vtm->TEMPLATE_Y_SIZE/4); + draw_image((const double*)&vtm->templates[vtm->current_vt].data[0], true, -1, 0.0, vtm->TEMPLATE_X_SIZE, + vtm->TEMPLATE_Y_SIZE, (double)vt_window_width / vtm->TEMPLATE_X_SIZE, + -(double)vt_window_height / vtm->TEMPLATE_Y_SIZE / 4); - draw_image((const double*)&vtm->current_view[0],true, -1.0, -0.5, - vtm->TEMPLATE_X_SIZE, vtm->TEMPLATE_Y_SIZE, - (double)vt_window_width/vtm->TEMPLATE_X_SIZE, - -(double)vt_window_height/vtm->TEMPLATE_Y_SIZE/4); + draw_image((const double*)&vtm->current_view[0], true, -1.0, -0.5, vtm->TEMPLATE_X_SIZE, vtm->TEMPLATE_Y_SIZE, + (double)vt_window_width / vtm->TEMPLATE_X_SIZE, -(double)vt_window_height / vtm->TEMPLATE_Y_SIZE / 4); view_template_scene->drawAll(); driver->endScene(); } - void update_ptr(LocalViewMatch *vt_in) + void update_ptr(LocalViewMatch* vt_in) { vtm = vt_in; } private: + void draw_image(const double* image, bool greyscale, float x, float y, int width, int height, double scale_x, + double scale_y) + { + unsigned char* texture_ptr_start = (unsigned char*)malloc(width * height * (greyscale ? 1 : 3)); - void draw_image(const double * image, bool greyscale, float x, float y, int width, int height, double scale_x, double scale_y) - { - unsigned char* texture_ptr_start = (unsigned char *) malloc(width*height * (greyscale ? 1 : 3)); - - const double * image_ptr = image; - const double * image_end = image_ptr + width * height * (greyscale ? 1 : 3); - unsigned char *texture_ptr = texture_ptr_start; + const double* image_ptr = image; + const double* image_end = image_ptr + width * height * (greyscale ? 1 : 3); + unsigned char* texture_ptr = texture_ptr_start; for (; image_ptr < image_end;) { *(texture_ptr++) = (unsigned char)(*(image_ptr++) * 255.0); @@ -110,28 +104,28 @@ class LocalViewScene draw_image(texture_ptr_start, greyscale, x, y, width, height, scale_x, scale_y); free(texture_ptr_start); - } - - void draw_image(const unsigned char * image, bool greyscale, float x, float y, int width, int height, double scale_x, double scale_y) - { - glRasterPos2f(x,y); - glPixelZoom(scale_x, scale_y); - if (greyscale) - glDrawPixels(width, height, GL_LUMINANCE, GL_UNSIGNED_BYTE, image); - else - glDrawPixels(width, height, GL_BGR, GL_UNSIGNED_BYTE, image); - } + } + void draw_image(const unsigned char* image, bool greyscale, float x, float y, int width, int height, double scale_x, + double scale_y) + { + glRasterPos2f(x, y); + glPixelZoom(scale_x, scale_y); + if (greyscale) + glDrawPixels(width, height, GL_LUMINANCE, GL_UNSIGNED_BYTE, image); + else + glDrawPixels(width, height, GL_BGR, GL_UNSIGNED_BYTE, image); + } - irr::IrrlichtDevice *device; - irr::video::IVideoDriver * driver; - irr::scene::ISceneManager * scene; - LocalViewMatch *vtm; - irr::scene::ISceneManager * view_template_scene; + irr::IrrlichtDevice* device; + irr::video::IVideoDriver* driver; + irr::scene::ISceneManager* scene; + LocalViewMatch* vtm; + irr::scene::ISceneManager* view_template_scene; int vt_window_width, vt_window_height; }; -}; // namespace ratslam +}; // namespace ratslam #endif /* VIEW_TEMPLATE_SCENE_HPP_ */ diff --git a/src/graphics/path_node.h b/src/graphics/path_node.h index 4997f4b..1e40807 100644 --- a/src/graphics/path_node.h +++ b/src/graphics/path_node.h @@ -34,145 +34,135 @@ class PathNode : public irr::scene::ISceneNode { public: - - PathNode(irr::scene::ISceneNode * parent) - : irr::scene::ISceneNode(parent, parent->getSceneManager()) - { - - } - - ~PathNode() - { - - } - - - virtual void OnRegisterSceneNode() - { - // if the node is visible then register it for - // rendering - if (IsVisible) - { - SceneManager->registerNodeForRendering(this); - } - - // register children - ISceneNode::OnRegisterSceneNode(); - } - - virtual void render() - { - // get the device - irr::video::IVideoDriver * driver = SceneManager->getVideoDriver(); - driver->setMaterial(mat); - driver->setTransform(irr::video::ETS_WORLD, AbsoluteTransformation); - - //std::vector::iterator it1 = vertices.begin(); - //std::vector::iterator it2; - - if (vertices.size() == 0) - { - return; - } - - irr::u32 primitive_count; - - switch(primitive_type) - { - case irr::scene::EPT_LINES: - primitive_count = indices.size() / 2; - break; - case irr::scene::EPT_POINTS: - primitive_count = indices.size(); - break; - case irr::scene::EPT_LINE_STRIP: - primitive_count = indices.size() - 1; - break; - default: - primitive_count = indices.size() / 3; - break; - } - - driver->drawVertexPrimitiveList((void*)&vertices[0], vertices.size(), - &indices[0], primitive_count, irr::video::EVT_STANDARD, this->primitive_type, - irr::video::EIT_32BIT); - - /*switch (primitive_type) - { - case irr::scene::EPT_LINES: - for (it2 = it1 + 1; it2 != points.end(); it1++, it2++) - { - if ((*it1).X == FLT_MAX || (*it2).X == FLT_MAX) - { - continue; - } - driver->draw3DLine(*it1, *it2, irr::video::SColor(255, 255, 0, 0)); - } - break; - - default: - - }*/ - - - - } - - virtual const irr::core::aabbox3d& getBoundingBox() const - { - // not actually using this, but return the box - // anyway - return box; - } - - virtual irr::u32 getMaterialCount() - { - // return the material count - return 1; - } - - virtual irr::video::SMaterial& getMaterial(irr::u32 i) - { - // return the material - return mat; - } - - void addPoint(const irr::core::vector3df & point) - { - // points.push_back(point); - irr::video::S3DVertex vertex; - vertex.Pos = point; - indices.push_back(vertices.size()); - vertices.push_back(vertex); - } - - void clearPoints() - { - indices.clear(); - vertices.clear(); - } - - /*void addGap() - { - irr::core::vector3df gap; - gap.X = FLT_MAX; - points.push_back(gap); - }*/ - - void setPrimitiveType(irr::scene::E_PRIMITIVE_TYPE primitive_type) - { - this->primitive_type = primitive_type; - } + PathNode(irr::scene::ISceneNode *parent) : irr::scene::ISceneNode(parent, parent->getSceneManager()) + { + } + + ~PathNode() + { + } + + virtual void OnRegisterSceneNode() + { + // if the node is visible then register it for + // rendering + if (IsVisible) + { + SceneManager->registerNodeForRendering(this); + } + + // register children + ISceneNode::OnRegisterSceneNode(); + } + + virtual void render() + { + // get the device + irr::video::IVideoDriver *driver = SceneManager->getVideoDriver(); + driver->setMaterial(mat); + driver->setTransform(irr::video::ETS_WORLD, AbsoluteTransformation); + + // std::vector::iterator it1 = vertices.begin(); + // std::vector::iterator it2; + + if (vertices.size() == 0) + { + return; + } + + irr::u32 primitive_count; + + switch (primitive_type) + { + case irr::scene::EPT_LINES: + primitive_count = indices.size() / 2; + break; + case irr::scene::EPT_POINTS: + primitive_count = indices.size(); + break; + case irr::scene::EPT_LINE_STRIP: + primitive_count = indices.size() - 1; + break; + default: + primitive_count = indices.size() / 3; + break; + } + + driver->drawVertexPrimitiveList((void *)&vertices[0], vertices.size(), &indices[0], primitive_count, + irr::video::EVT_STANDARD, this->primitive_type, irr::video::EIT_32BIT); + + /*switch (primitive_type) + { + case irr::scene::EPT_LINES: + for (it2 = it1 + 1; it2 != points.end(); it1++, it2++) + { + if ((*it1).X == FLT_MAX || (*it2).X == FLT_MAX) + { + continue; + } + driver->draw3DLine(*it1, *it2, irr::video::SColor(255, 255, 0, 0)); + } + break; + + default: + + }*/ + } + + virtual const irr::core::aabbox3d &getBoundingBox() const + { + // not actually using this, but return the box + // anyway + return box; + } + + virtual irr::u32 getMaterialCount() + { + // return the material count + return 1; + } + + virtual irr::video::SMaterial &getMaterial(irr::u32 i) + { + // return the material + return mat; + } + + void addPoint(const irr::core::vector3df &point) + { + // points.push_back(point); + irr::video::S3DVertex vertex; + vertex.Pos = point; + indices.push_back(vertices.size()); + vertices.push_back(vertex); + } + + void clearPoints() + { + indices.clear(); + vertices.clear(); + } + + /*void addGap() + { + irr::core::vector3df gap; + gap.X = FLT_MAX; + points.push_back(gap); + }*/ + + void setPrimitiveType(irr::scene::E_PRIMITIVE_TYPE primitive_type) + { + this->primitive_type = primitive_type; + } private: - // scene node variables - irr::core::aabbox3d box; - irr::video::SMaterial mat; - // std::vector points; - std::vector vertices; - std::vector indices; - irr::scene::E_PRIMITIVE_TYPE primitive_type; + // scene node variables + irr::core::aabbox3d box; + irr::video::SMaterial mat; + // std::vector points; + std::vector vertices; + std::vector indices; + irr::scene::E_PRIMITIVE_TYPE primitive_type; }; - #endif /* PATHGRAPHNODE_H_ */ diff --git a/src/graphics/posecell_scene.h b/src/graphics/posecell_scene.h index 631fa76..2699c69 100644 --- a/src/graphics/posecell_scene.h +++ b/src/graphics/posecell_scene.h @@ -36,39 +36,41 @@ namespace ratslam { - class PosecellScene { public: - PosecellScene(ptree & settings, PosecellNetwork *in_pc) : - pose_cells_scene(NULL), particles(NULL), position_line(NULL), pose_cell_history(NULL), posecells(in_pc) + PosecellScene(ptree& settings, PosecellNetwork* in_pc) + : pose_cells_scene(NULL), particles(NULL), position_line(NULL), pose_cell_history(NULL), posecells(in_pc) { - window_width = 400; - window_height = 400; + window_width = 400; + window_height = 400; - device = irr::createDevice(irr::video::EDT_OPENGL, irr::core::dimension2d(window_width, window_height), 32, false, false, false); + device = irr::createDevice(irr::video::EDT_OPENGL, irr::core::dimension2d(window_width, window_height), + 32, false, false, false); device->setWindowCaption(L"openRatSLAM Pose Cell Network"); driver = device->getVideoDriver(); scene = device->getSceneManager(); - get_setting_from_ptree(media_path, settings, "media_path", (std::string)""); + get_setting_from_ptree(media_path, settings, "media_path", (std::string) ""); pose_cells_scene = scene->createNewSceneManager(false); - particles = new irr::scene::IBillboardSceneNode*[NUM_PARTICLES]; + particles = new irr::scene::IBillboardSceneNode* [NUM_PARTICLES]; for (int i = 0; i < NUM_PARTICLES; i++) { - particles[i] = pose_cells_scene->addBillboardSceneNode(pose_cells_scene->getRootSceneNode(), irr::core::dimension2d(2, 2)); + particles[i] = pose_cells_scene->addBillboardSceneNode(pose_cells_scene->getRootSceneNode(), + irr::core::dimension2d(2, 2)); particles[i]->setMaterialFlag(irr::video::EMF_LIGHTING, false); particles[i]->getMaterial(0).EmissiveColor = irr::video::SColor(255, 255, 255, 255); particles[i]->setMaterialType(irr::video::EMT_TRANSPARENT_ADD_COLOR); - particles[i]->setMaterialTexture(0, pose_cells_scene->getVideoDriver()->getTexture((media_path + "/particle.bmp").c_str())); - + particles[i]->setMaterialTexture( + 0, pose_cells_scene->getVideoDriver()->getTexture((media_path + "/particle.bmp").c_str())); } - position_line = pose_cells_scene->addMeshSceneNode(pose_cells_scene->getGeometryCreator()->createCylinderMesh(0.5, 1.0, 5, irr::video::SColor(255, 255, 0, 0)), - pose_cells_scene->getRootSceneNode()); + position_line = pose_cells_scene->addMeshSceneNode( + pose_cells_scene->getGeometryCreator()->createCylinderMesh(0.5, 1.0, 5, irr::video::SColor(255, 255, 0, 0)), + pose_cells_scene->getRootSceneNode()); position_line->getMaterial(0).Lighting = false; position_line->getMaterial(0).EmissiveColor = irr::video::SColor(255, 255, 0, 0); @@ -76,54 +78,77 @@ class PosecellScene pose_cell_history->getMaterial(0).EmissiveColor = irr::video::SColor(255, 0, 255, 0); pose_cell_history->setPrimitiveType(irr::scene::EPT_POINTS); - PathNode * pose_cell_boundary = new PathNode(pose_cells_scene->getRootSceneNode()); + PathNode* pose_cell_boundary = new PathNode(pose_cells_scene->getRootSceneNode()); pose_cell_boundary->setPrimitiveType(irr::scene::EPT_LINES); pose_cell_boundary->getMaterial(0).EmissiveColor = irr::video::SColor(255, 255, 255, 255); pose_cell_boundary->getMaterial(0).Thickness = 2.0f; // bottom - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); // top - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); // sides - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); - pose_cell_boundary->addPoint(irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); - - irr::scene::ICameraSceneNode * pose_cell_camera = pose_cells_scene->addCameraSceneNode(NULL, irr::core::vector3df(10, 40, -40), irr::core::vector3df(0, 0, 0)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(-posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, -posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + pose_cell_boundary->addPoint( + irr::core::vector3df(posecells->PC_DIM_XY / 2.0f, posecells->PC_DIM_TH / 2.0f, -posecells->PC_DIM_XY / 2.0f)); + + irr::scene::ICameraSceneNode* pose_cell_camera = + pose_cells_scene->addCameraSceneNode(NULL, irr::core::vector3df(10, 40, -40), irr::core::vector3df(0, 0, 0)); irr::core::matrix4 proj; - proj.buildProjectionMatrixOrthoLH((irr::f32)posecells->PC_DIM_XY * 2 + 1, (irr::f32)50, (irr::f32)0.01, (irr::f32)100); + proj.buildProjectionMatrixOrthoLH((irr::f32)posecells->PC_DIM_XY * 2 + 1, (irr::f32)50, (irr::f32)0.01, + (irr::f32)100); pose_cell_camera->setProjectionMatrix(proj, true); - - } ~PosecellScene() { - } - void update_pose_cells(double * pose_cells, int PC_DIM_XY, int PC_DIM_TH) + void update_pose_cells(double* pose_cells, int PC_DIM_XY, int PC_DIM_TH) { irr::core::matrix4 mat; irr::core::vector3df trans; @@ -140,8 +165,9 @@ class PosecellScene { if (next_particle < NUM_PARTICLES) { - particles[next_particle]->setPosition( - irr::core::vector3df((irr::f32)(i - posecells->PC_DIM_XY / 2), (irr::f32)(k - posecells->PC_DIM_TH / 2), (irr::f32)(j - posecells->PC_DIM_XY / 2))); + particles[next_particle]->setPosition(irr::core::vector3df((irr::f32)(i - posecells->PC_DIM_XY / 2), + (irr::f32)(k - posecells->PC_DIM_TH / 2), + (irr::f32)(j - posecells->PC_DIM_XY / 2))); particles[next_particle]->setVisible(true); next_particle++; } @@ -159,18 +185,23 @@ class PosecellScene void update_position(double x, double y, double th, int PC_DIM_XY, int PC_DIM_TH) { - position_line->setPosition(irr::core::vector3df((irr::f32)(x - (double)PC_DIM_XY / 2), (irr::f32)(th - (double)PC_DIM_TH / 2), (irr::f32)(y - (double)PC_DIM_XY / 2))); + position_line->setPosition(irr::core::vector3df((irr::f32)(x - (double)PC_DIM_XY / 2), + (irr::f32)(th - (double)PC_DIM_TH / 2), + (irr::f32)(y - (double)PC_DIM_XY / 2))); - position_line->setScale(irr::core::vector3df((irr::f32)0.5, (irr::f32)-th, (irr::f32)0.5)); + position_line->setScale(irr::core::vector3df((irr::f32)0.5, (irr::f32) - th, (irr::f32)0.5)); // TODO free points after eg 1000 points - pose_cell_history->addPoint(irr::core::vector3df((irr::f32)(x - (double)PC_DIM_XY / 2), (irr::f32)(-(double)PC_DIM_TH / 2), (irr::f32)(y - (double)PC_DIM_XY / 2))); + pose_cell_history->addPoint(irr::core::vector3df((irr::f32)(x - (double)PC_DIM_XY / 2), + (irr::f32)(-(double)PC_DIM_TH / 2), + (irr::f32)(y - (double)PC_DIM_XY / 2))); } void update_scene() { update_pose_cells(posecells->posecells_memory, posecells->PC_DIM_XY, posecells->PC_DIM_TH); - update_position(posecells->best_x, posecells->best_y, posecells->best_th, posecells->PC_DIM_XY, posecells->PC_DIM_TH); + update_position(posecells->best_x, posecells->best_y, posecells->best_th, posecells->PC_DIM_XY, + posecells->PC_DIM_TH); } void clear_history() @@ -180,13 +211,13 @@ class PosecellScene void draw_all() { - device->run(); // TODO return the bool for quiting + device->run(); // TODO return the bool for quiting driver->beginScene(true, true, irr::video::SColor(255, 0, 0, 0)); pose_cells_scene->drawAll(); driver->endScene(); } - void update_ptr(PosecellNetwork *pc_in) + void update_ptr(PosecellNetwork* pc_in) { clear_history(); posecells = pc_in; @@ -194,24 +225,21 @@ class PosecellScene private: static const int NUM_PARTICLES = 500; - irr::scene::ISceneManager * pose_cells_scene; - irr::scene::IBillboardSceneNode ** particles; - irr::scene::IMeshSceneNode * position_line; - PathNode * pose_cell_history; - PosecellNetwork *posecells; + irr::scene::ISceneManager* pose_cells_scene; + irr::scene::IBillboardSceneNode** particles; + irr::scene::IMeshSceneNode* position_line; + PathNode* pose_cell_history; + PosecellNetwork* posecells; - irr::IrrlichtDevice *device; - irr::video::IVideoDriver * driver; - irr::scene::ISceneManager * scene; + irr::IrrlichtDevice* device; + irr::video::IVideoDriver* driver; + irr::scene::ISceneManager* scene; std::string media_path; - unsigned int window_width, window_height; }; - -} -; +}; // namespace ratslam #endif /* POSECELLSCENE_HPP_ */ diff --git a/src/main_em.cpp b/src/main_em.cpp index f221261..9e51bda 100644 --- a/src/main_em.cpp +++ b/src/main_em.cpp @@ -29,8 +29,12 @@ #include "utils/utils.h" #include +#include +#include #include +#include +#include #include "ratslam/experience_map.h" #include @@ -43,25 +47,53 @@ #include +// pubs ros::Publisher pub_em; ros::Publisher pub_pose; ros::Publisher pub_em_markers; ros::Publisher pub_goal_path; + +// tf +tf::TransformListener* tf_listener; +tf::TransformBroadcaster* tf_broadcaster; +tf::Transform map_to_odom; + +// odom +double odom_pose[3] = { 0.0, 0.0, 0.0 }; +double mpose[3] = { 0.0, 0.0, 0.0 }; + geometry_msgs::PoseStamped pose_output; ratslam_ros::TopologicalMap em_map; visualization_msgs::Marker em_marker; +ratslam::ExperienceMap* em; #ifdef HAVE_IRRLICHT + #include "graphics/experience_map_scene.h" -ratslam::ExperienceMapScene *ems; + +ratslam::ExperienceMapScene* ems; bool use_graphics; #endif using namespace ratslam; -void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::ExperienceMap *em) +std::string map_frame = std::string("map"); +std::string odom_frame = std::string("odom"); +std::string base_frame = std::string("base_link"); +double tf_update_rate = 20.0; +double map_save_period = 10.0; +std::string map_file_path = std::string("ratslam-latest.bmap"); + +/** + * \brief The Odometry callback function. + * + * \param odo The incoming odometry message. + * \param em Pointer to current experience map. + */ +void odo_callback(nav_msgs::OdometryConstPtr odo) { - ROS_DEBUG_STREAM("EM:odo_callback{" << ros::Time::now() << "} seq=" << odo->header.seq << " v=" << odo->twist.twist.linear.x << " r=" << odo->twist.twist.angular.z); + ROS_DEBUG_STREAM("EM:odo_callback{" << ros::Time::now() << "} seq=" << odo->header.seq + << " v=" << odo->twist.twist.linear.x << " r=" << odo->twist.twist.angular.z); static ros::Time prev_time(0); @@ -76,7 +108,7 @@ void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::ExperienceMap *em) if (em->get_current_goal_id() >= 0) { // (prev_goal_update.toSec() == 0 || (odo->header.stamp - prev_goal_update).toSec() > 5) - //em->calculate_path_to_goal(odo->header.stamp.toSec()); + // em->calculate_path_to_goal(odo->header.stamp.toSec()); prev_goal_update = odo->header.stamp; @@ -89,10 +121,10 @@ void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::ExperienceMap *em) static geometry_msgs::PoseStamped pose; path.header.stamp = ros::Time::now(); - path.header.frame_id = "1"; + path.header.frame_id = map_frame; pose.header.seq = 0; - pose.header.frame_id = "1"; + pose.header.frame_id = map_frame; path.poses.clear(); unsigned int trace_exp_id = em->get_goals()[0]; while (trace_exp_id != em->get_goal_path_final_exp()) @@ -108,12 +140,11 @@ void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::ExperienceMap *em) pub_goal_path.publish(path); path.header.seq++; - } else { path.header.stamp = ros::Time::now(); - path.header.frame_id = "1"; + path.header.frame_id = map_frame; path.poses.clear(); pub_goal_path.publish(path); @@ -124,9 +155,10 @@ void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::ExperienceMap *em) prev_time = odo->header.stamp; } -void action_callback(ratslam_ros::TopologicalActionConstPtr action, ratslam::ExperienceMap *em) +void action_callback(ratslam_ros::TopologicalActionConstPtr action) { - ROS_DEBUG_STREAM("EM:action_callback{" << ros::Time::now() << "} action=" << action->action << " src=" << action->src_id << " dst=" << action->dest_id); + ROS_DEBUG_STREAM("EM:action_callback{" << ros::Time::now() << "} action=" << action->action + << " src=" << action->src_id << " dst=" << action->dest_id); switch (action->action) { @@ -144,13 +176,16 @@ void action_callback(ratslam_ros::TopologicalActionConstPtr action, ratslam::Exp em->on_set_experience(action->dest_id, action->relative_rad); break; + default: + ROS_WARN("Received invalid action!"); + break; } em->iterate(); pose_output.header.stamp = ros::Time::now(); pose_output.header.seq++; - pose_output.header.frame_id = "1"; + pose_output.header.frame_id = map_frame; pose_output.pose.position.x = em->get_experience(em->get_current_id())->x_m; pose_output.pose.position.y = em->get_experience(em->get_current_id())->y_m; pose_output.pose.position.z = 0; @@ -160,6 +195,10 @@ void action_callback(ratslam_ros::TopologicalActionConstPtr action, ratslam::Exp pose_output.pose.orientation.w = cos(em->get_experience(em->get_current_id())->th_rad / 2.0); pub_pose.publish(pose_output); + mpose[0] = em->get_experience(em->get_current_id())->x_m; + mpose[1] = em->get_experience(em->get_current_id())->y_m; + mpose[2] = em->get_experience(em->get_current_id())->th_rad; + static ros::Time prev_pub_time(0); if (action->header.stamp - prev_pub_time > ros::Duration(30.0)) @@ -200,13 +239,13 @@ void action_callback(ratslam_ros::TopologicalActionConstPtr action, ratslam::Exp em_marker.header.stamp = ros::Time::now(); em_marker.header.seq++; - em_marker.header.frame_id = "1"; + em_marker.header.frame_id = map_frame; em_marker.type = visualization_msgs::Marker::LINE_LIST; em_marker.points.resize(em->get_num_links() * 2); em_marker.action = visualization_msgs::Marker::ADD; em_marker.scale.x = 0.01; - //em_marker.scale.y = 1; - //em_marker.scale.z = 1; + // em_marker.scale.y = 1; + // em_marker.scale.z = 1; em_marker.color.a = 1; em_marker.ns = "em"; em_marker.id = 0; @@ -215,7 +254,6 @@ void action_callback(ratslam_ros::TopologicalActionConstPtr action, ratslam::Exp em_marker.pose.orientation.z = 0; em_marker.pose.orientation.w = 1; for (int i = 0; i < em->get_num_links(); i++) - { em_marker.points[i * 2].x = em->get_experience(em->get_link(i)->exp_from_id)->x_m; em_marker.points[i * 2].y = em->get_experience(em->get_link(i)->exp_from_id)->y_m; @@ -236,53 +274,143 @@ void action_callback(ratslam_ros::TopologicalActionConstPtr action, ratslam::Exp #endif } -void set_goal_pose_callback(geometry_msgs::PoseStampedConstPtr pose, ratslam::ExperienceMap * em) +void set_goal_pose_callback(geometry_msgs::PoseStampedConstPtr pose) { em->add_goal(pose->pose.position.x, pose->pose.position.y); +} +void tf_update_timer_callback(const ros::TimerEvent& event) +{ + tf::StampedTransform odom_to_base_; + try + { + tf_listener->lookupTransform(odom_frame, base_frame, ros::Time(0), odom_to_base_); + } + catch (tf::TransformException& e) + { + ROS_WARN("Failed to compute odom pose. Exception: %s", e.what()); + return; + } + tf::Transform odom_to_base = odom_to_base_; + + tf::Transform base_to_map = + tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose[2]), tf::Vector3(mpose[0], mpose[1], 0.0)).inverse(); + + map_to_odom = (odom_to_base * base_to_map).inverse(); + + ros::Time tf_expiration = ros::Time::now() + ros::Duration(0.5); + tf_broadcaster->sendTransform(tf::StampedTransform(map_to_odom, tf_expiration, map_frame, odom_frame)); } -int main(int argc, char * argv[]) +void save_map_timer_callback(const ros::TimerEvent& event) { - ROS_INFO_STREAM(argv[0] << " - openRatSLAM Copyright (C) 2012 David Ball and Scott Heath"); - ROS_INFO_STREAM("RatSLAM algorithm by Michael Milford and Gordon Wyeth"); - ROS_INFO_STREAM("Distributed under the GNU GPL v3, see the included license file."); + // create and open a character archive for output + std::ofstream ofs(map_file_path.c_str()); + + // save map to archive file + { + boost::archive::binary_oarchive binary_archive(ofs); + // write class instance to archive + binary_archive << *em; + // archive and stream closed when destructors are called + } +} + +bool load_map(const std::string& file_path) +{ + try + { + // create and open an archive for input + std::ifstream ifs(file_path.c_str()); + boost::archive::binary_iarchive binary_archive(ifs); + // read class state from archive + binary_archive >> *em; + // archive and stream closed when destructors are called + return true; + } + catch (...) + { + return false; + } +} + +int main(int argc, char* argv[]) +{ + // print info without ROS because it is not initialized yet + std::cout << argv[0] << " - openRatSLAM Copyright (C) 2012 David Ball and Scott Heath"; + std::cout << "RatSLAM algorithm by Michael Milford and Gordon Wyeth"; + std::cout << "Distributed under the GNU GPL v3, see the included license file."; if (argc < 2) { - ROS_FATAL_STREAM("USAGE: " << argv[0] << " "); + std::cout << "USAGE: " << argv[0] << " "; exit(-1); } + + // initialize ROS node + ros::init(argc, argv, "RatSLAMExperienceMap"); + std::string topic_root = ""; boost::property_tree::ptree settings, general_settings, ratslam_settings; read_ini(argv[1], settings); get_setting_child(ratslam_settings, settings, "ratslam", true); get_setting_child(general_settings, settings, "general", true); - get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string)""); + // backward compatibility, namespace or private nodehandle should do it more ROS like + get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string) ""); - if (!ros::isInitialized()) + ros::NodeHandle node; + ros::NodeHandle priv_node("~"); + + // setup tf + tf::TransformListener tf_listener_; + tf::TransformBroadcaster tf_broadcaster_; + tf_listener = &tf_listener_; + tf_broadcaster = &tf_broadcaster_; + + // params + priv_node.param("map_frame", map_frame, map_frame); + priv_node.param("odom_frame", odom_frame, odom_frame); + priv_node.param("base_frame", base_frame, base_frame); + priv_node.param("tf_update_rate", tf_update_rate, tf_update_rate); + priv_node.param("map_save_period", map_save_period, map_save_period); + priv_node.param("map_file_path", map_file_path, map_file_path); + + // check backward compatibility with configs that have topic_root set + std::string odom_topic = "odom"; + if (!topic_root.empty()) { - ros::init(argc, argv, "RatSLAMExperienceMap"); + odom_topic = topic_root + "/odom"; } - ros::NodeHandle node; - ratslam::ExperienceMap * em = new ratslam::ExperienceMap(ratslam_settings); + // create the experience map object + em = new ratslam::ExperienceMap(ratslam_settings); + // try to load map + if (!load_map(map_file_path)) + { + ROS_WARN("Could not load map from file \"%s\"", map_file_path.c_str()); + } + + // pubs pub_em = node.advertise(topic_root + "/ExperienceMap/Map", 1); pub_em_markers = node.advertise(topic_root + "/ExperienceMap/MapMarker", 1); - pub_pose = node.advertise(topic_root + "/ExperienceMap/RobotPose", 1); - pub_goal_path = node.advertise(topic_root + "/ExperienceMap/PathToGoal", 1); - ros::Subscriber sub_odometry = node.subscribe(topic_root + "/odom", 0, boost::bind(odo_callback, _1, em), ros::VoidConstPtr(), + // subs + ros::Subscriber sub_odometry = node.subscribe(odom_topic, 0, odo_callback, ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); - ros::Subscriber sub_action = node.subscribe(topic_root + "/PoseCell/TopologicalAction", 0, boost::bind(action_callback, _1, em), - ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub_action = + node.subscribe(topic_root + "/PoseCell/TopologicalAction", 0, action_callback, + ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub_goal = + node.subscribe(topic_root + "/ExperienceMap/SetGoalPose", 0, set_goal_pose_callback, + ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); - ros::Subscriber sub_goal = node.subscribe(topic_root + "/ExperienceMap/SetGoalPose", 0, boost::bind(set_goal_pose_callback, _1, em), - ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); + // timers + ros::Timer tf_update_timer = priv_node.createTimer(ros::Duration(1.0 / tf_update_rate), tf_update_timer_callback); + ros::Timer save_map_timer = priv_node.createTimer(ros::Duration(map_save_period), save_map_timer_callback); #ifdef HAVE_IRRLICHT boost::property_tree::ptree draw_settings; @@ -298,4 +426,3 @@ int main(int argc, char * argv[]) return 0; } - diff --git a/src/main_lv.cpp b/src/main_lv.cpp index a9b63d9..ba58ebe 100644 --- a/src/main_lv.cpp +++ b/src/main_lv.cpp @@ -27,6 +27,10 @@ */ #include + +#include +#include + using namespace std; #include @@ -47,29 +51,34 @@ using namespace std; #include "ratslam/local_view_match.h" #if HAVE_IRRLICHT + #include "graphics/local_view_scene.h" -ratslam::LocalViewScene *lvs = NULL; + +ratslam::LocalViewScene* lvs = NULL; bool use_graphics; #endif +ros::Publisher pub_vt; using namespace ratslam; -ratslam::LocalViewMatch * lv = NULL; +ratslam::LocalViewMatch* lv = NULL; +std::string lvm_file_path = std::string("ratslam-latest.blvm"); +double lvm_save_period = 10.0; -void image_callback(sensor_msgs::ImageConstPtr image, ros::Publisher * pub_vt) +void image_callback(sensor_msgs::ImageConstPtr image) { ROS_DEBUG_STREAM("LV:image_callback{" << ros::Time::now() << "} seq=" << image->header.seq); static ratslam_ros::ViewTemplate vt_output; - lv->on_image(&image->data[0], (image->encoding == "bgr8" ? false : true), image->width, image->height); + lv->on_image(&image->data[0], !(image->encoding == "bgr8"), image->width, image->height); vt_output.header.stamp = ros::Time::now(); vt_output.header.seq++; vt_output.current_id = lv->get_current_vt(); vt_output.relative_rad = lv->get_relative_rad(); - pub_vt->publish(vt_output); + pub_vt.publish(vt_output); #ifdef HAVE_IRRLICHT if (use_graphics) @@ -79,7 +88,39 @@ void image_callback(sensor_msgs::ImageConstPtr image, ros::Publisher * pub_vt) #endif } -int main(int argc, char * argv[]) +void save_lvm_timer_callback(const ros::TimerEvent& event) +{ + // create and open a character archive for output + std::ofstream ofs(lvm_file_path.c_str()); + + // save map to archive file + { + boost::archive::binary_oarchive binary_archive(ofs); + // write class instance to archive + binary_archive << *lv; + // archive and stream closed when destructors are called + } +} + +bool load_lvm(const std::string& file_path) +{ + try + { + // create and open an archive for input + std::ifstream ifs(file_path.c_str()); + boost::archive::binary_iarchive binary_archive(ifs); + // read class state from archive + binary_archive >> *lv; + // archive and stream closed when destructors are called + return true; + } + catch (...) + { + return false; + } +} + +int main(int argc, char* argv[]) { ROS_INFO_STREAM(argv[0] << " - openRatSLAM Copyright (C) 2012 David Ball and Scott Heath"); ROS_INFO_STREAM("RatSLAM algorithm by Michael Milford and Gordon Wyeth"); @@ -96,28 +137,53 @@ int main(int argc, char * argv[]) read_ini(argv[1], settings); get_setting_child(general_settings, settings, "general", true); - get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string)""); + // backward compatibility, namespace or private nodehandle should do it more ROS like + get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string) ""); get_setting_child(ratslam_settings, settings, "ratslam", true); + + // initialize ROS node + ros::init(argc, argv, "RatSLAMViewTemplate"); + + // setup public and private nodehandles + ros::NodeHandle node; + ros::NodeHandle priv_node("~"); + + // params + priv_node.param("lvm_save_period", lvm_save_period, lvm_save_period); + priv_node.param("lvm_file_path", lvm_file_path, lvm_file_path); + + // check backward compatibility with configs that have topic_root set + std::string image_topic = "image"; + if (!topic_root.empty()) + { + // prepend with topic with topic_root setting + image_topic = topic_root + "/camera/image"; + } + lv = new ratslam::LocalViewMatch(ratslam_settings); - if (!ros::isInitialized()) + // try to load lvm + if (!load_lvm(lvm_file_path)) { - ros::init(argc, argv, "RatSLAMViewTemplate"); + ROS_WARN("Could not load LocalViewMatch from file \"%s\"", lvm_file_path.c_str()); } - ros::NodeHandle node; - ros::Publisher pub_vt = node.advertise(topic_root + "/LocalView/Template", 0); + // pubs + pub_vt = node.advertise(topic_root + "/LocalView/Template", 0); + // image transport image_transport::ImageTransport it(node); - image_transport::Subscriber sub = it.subscribe(topic_root + "/camera/image", 0, boost::bind(image_callback, _1, &pub_vt)); + image_transport::Subscriber sub = it.subscribe(image_topic, 0, image_callback); + // timers + ros::Timer lvm_save_timer = node.createTimer(ros::Duration(lvm_save_period), save_lvm_timer_callback); #ifdef HAVE_IRRLICHT - boost::property_tree::ptree draw_settings; - get_setting_child(draw_settings, settings, "draw", true); - get_setting_from_ptree(use_graphics, draw_settings, "enable", true); - if (use_graphics) - lvs = new ratslam::LocalViewScene(draw_settings, lv); + boost::property_tree::ptree draw_settings; + get_setting_child(draw_settings, settings, "draw", true); + get_setting_from_ptree(use_graphics, draw_settings, "enable", true); + if (use_graphics) + lvs = new ratslam::LocalViewScene(draw_settings, lv); #endif ros::spin(); diff --git a/src/main_pc.cpp b/src/main_pc.cpp index 0411d19..f632c21 100644 --- a/src/main_pc.cpp +++ b/src/main_pc.cpp @@ -26,33 +26,41 @@ * along with this program. If not, see . */ #include -using namespace std; - -#include "utils/utils.h" #include +#include +#include #include - -#include "ratslam/posecell_network.h" -#include #include +#include #include +#include "utils/utils.h" +#include "ratslam/posecell_network.h" #if HAVE_IRRLICHT + #include "graphics/posecell_scene.h" -ratslam::PosecellScene *pcs; + +ratslam::PosecellScene* pcs; bool use_graphics; #endif +using namespace std; using namespace ratslam; +ratslam::PosecellNetwork* pc; +ros::Publisher pub_pc; + ratslam_ros::TopologicalAction pc_output; +std::string pcn_file_path = std::string("ratslam-latest.bpcn"); +double pcn_save_period = 10.0; -void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::PosecellNetwork *pc, ros::Publisher * pub_pc) +void odo_callback(nav_msgs::OdometryConstPtr odo) { - ROS_DEBUG_STREAM("PC:odo_callback{" << ros::Time::now() << "} seq=" << odo->header.seq << " v=" << odo->twist.twist.linear.x << " r=" << odo->twist.twist.angular.z); + ROS_DEBUG_STREAM("PC:odo_callback{" << ros::Time::now() << "} seq=" << odo->header.seq + << " v=" << odo->twist.twist.linear.x << " r=" << odo->twist.twist.angular.z); static ros::Time prev_time(0); @@ -68,40 +76,73 @@ void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::PosecellNetwork *pc, pc_output.header.stamp = ros::Time::now(); pc_output.header.seq++; pc_output.dest_id = pc->get_current_exp_id(); - pc_output.relative_rad = pc->get_relative_rad(); - pub_pc->publish(pc_output); - ROS_DEBUG_STREAM("PC:action_publish{odo}{" << ros::Time::now() << "} action{" << pc_output.header.seq << "}=" << pc_output.action << " src=" << pc_output.src_id << " dest=" << pc_output.dest_id); + pc_output.relative_rad = pc->get_relative_rad(); + pub_pc.publish(pc_output); + ROS_DEBUG_STREAM("PC:action_publish{odo}{" << ros::Time::now() << "} action{" << pc_output.header.seq + << "}=" << pc_output.action << " src=" << pc_output.src_id + << " dest=" << pc_output.dest_id); } - #ifdef HAVE_IRRLICHT - if (use_graphics) - { - pcs->update_scene(); - pcs->draw_all(); - } + if (use_graphics) + { + pcs->update_scene(); + pcs->draw_all(); + } #endif } prev_time = odo->header.stamp; } -void template_callback(ratslam_ros::ViewTemplateConstPtr vt, ratslam::PosecellNetwork *pc, ros::Publisher * pub_pc) +void template_callback(ratslam_ros::ViewTemplateConstPtr vt) { - ROS_DEBUG_STREAM("PC:vt_callback{" << ros::Time::now() << "} seq=" << vt->header.seq << " id=" << vt->current_id << " rad=" << vt->relative_rad); + ROS_DEBUG_STREAM("PC:vt_callback{" << ros::Time::now() << "} seq=" << vt->header.seq << " id=" << vt->current_id + << " rad=" << vt->relative_rad); pc->on_view_template(vt->current_id, vt->relative_rad); #ifdef HAVE_IRRLICHT - if (use_graphics) - { - pcs->update_scene(); - pcs->draw_all(); - } + if (use_graphics) + { + pcs->update_scene(); + pcs->draw_all(); + } #endif +} +void save_pcn_timer_callback(const ros::TimerEvent& event) +{ + // create and open a character archive for output + std::ofstream ofs(pcn_file_path.c_str()); + + // save map to archive file + { + boost::archive::binary_oarchive binary_archive(ofs); + // write class instance to archive + pc->save(binary_archive, 1); + // archive and stream closed when destructors are called + } +} + +bool load_pcn(const std::string& file_path) +{ + try + { + // create and open an archive for input + std::ifstream ifs(file_path.c_str()); + boost::archive::binary_iarchive binary_archive(ifs); + // read class state from archive + pc->load(binary_archive, 1); + // archive and stream closed when destructors are called + return true; + } + catch (...) + { + return false; + } } -int main(int argc, char * argv[]) +int main(int argc, char* argv[]) { ROS_INFO_STREAM(argv[0] << " - openRatSLAM Copyright (C) 2012 David Ball and Scott Heath"); ROS_INFO_STREAM("RatSLAM algorithm by Michael Milford and Gordon Wyeth"); @@ -120,28 +161,53 @@ int main(int argc, char * argv[]) get_setting_child(general_settings, settings, "general", true); get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string) ""); - if (!ros::isInitialized()) + // initialize ROS node + ros::init(argc, argv, "RatSLAMPoseCells"); + + // setup public and private nodehandles + ros::NodeHandle node; + ros::NodeHandle priv_node("~"); + + // params + priv_node.param("pcn_save_period", pcn_save_period, pcn_save_period); + priv_node.param("pcn_file_path", pcn_file_path, pcn_file_path); + + // check backward compatibility with configs that have topic_root set + std::string odom_topic = "odom"; + if (!topic_root.empty()) { - ros::init(argc, argv, "RatSLAMPoseCells"); + odom_topic = topic_root + "/odom"; } - ros::NodeHandle node; + // create PoseCellNetwork object + pc = new ratslam::PosecellNetwork(ratslam_settings); + // try to load pcn + if (!load_pcn(pcn_file_path)) + { + ROS_WARN("Could not load PoseCellNetwork from file \"%s\"", pcn_file_path.c_str()); + } - ratslam::PosecellNetwork * pc = new ratslam::PosecellNetwork(ratslam_settings); - ros::Publisher pub_pc = node.advertise(topic_root + "/PoseCell/TopologicalAction", 0); + // pubs + pub_pc = node.advertise(topic_root + "/PoseCell/TopologicalAction", 0); - ros::Subscriber sub_odometry = node.subscribe(topic_root + "/odom", 0, boost::bind(odo_callback, _1, pc, &pub_pc), ros::VoidConstPtr(), + // subs + ros::Subscriber sub_odometry = node.subscribe(odom_topic, 0, odo_callback, ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); - ros::Subscriber sub_template = node.subscribe(topic_root + "/LocalView/Template", 0, boost::bind(template_callback, _1, pc, &pub_pc), - ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub_template = + node.subscribe(topic_root + "/LocalView/Template", 0, template_callback, + ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); + + // timers + ros::Timer pcn_save_timer = node.createTimer(ros::Duration(pcn_save_period), save_pcn_timer_callback); + #ifdef HAVE_IRRLICHT boost::property_tree::ptree draw_settings; get_setting_child(draw_settings, settings, "draw", true); get_setting_from_ptree(use_graphics, draw_settings, "enable", true); if (use_graphics) { - pcs = new ratslam::PosecellScene(draw_settings, pc); + pcs = new ratslam::PosecellScene(draw_settings, pc); } #endif diff --git a/src/main_vo.cpp b/src/main_vo.cpp index 3e3ec3d..c3eacb5 100644 --- a/src/main_vo.cpp +++ b/src/main_vo.cpp @@ -27,6 +27,7 @@ */ #include + using namespace std; #include "utils/utils.h" @@ -44,8 +45,7 @@ using namespace std; #include - -ratslam::VisualOdometry *vo = NULL; +ratslam::VisualOdometry* vo = NULL; ros::Publisher pub_vo; using namespace ratslam; @@ -56,7 +56,8 @@ void image_callback(sensor_msgs::ImageConstPtr image) static nav_msgs::Odometry odom_output; - vo->on_image(&image->data[0], (image->encoding == "bgr8" ? false : true), image->width, image->height, &odom_output.twist.twist.linear.x, &odom_output.twist.twist.angular.z); + vo->on_image(&image->data[0], (image->encoding == "bgr8" ? false : true), image->width, image->height, + &odom_output.twist.twist.linear.x, &odom_output.twist.twist.angular.z); odom_output.header.stamp = image->header.stamp; odom_output.header.seq++; @@ -64,8 +65,7 @@ void image_callback(sensor_msgs::ImageConstPtr image) pub_vo.publish(odom_output); } - -int main(int argc, char * argv[]) +int main(int argc, char* argv[]) { ROS_INFO_STREAM(argv[0] << " - openRatSLAM Copyright (C) 2012 David Ball and Scott Heath"); ROS_INFO_STREAM("RatSLAM algorithm by Michael Milford and Gordon Wyeth"); @@ -83,6 +83,7 @@ int main(int argc, char * argv[]) read_ini(argv[1], settings); ratslam::get_setting_child(vo_settings, settings, "visual_odometry", true); ratslam::get_setting_child(general_settings, settings, "general", true); + // backward compatibility, namespace or private nodehandle should do it more ROS like ratslam::get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string) ""); vo = new ratslam::VisualOdometry(vo_settings); @@ -93,10 +94,22 @@ int main(int argc, char * argv[]) } ros::NodeHandle node; - pub_vo = node.advertise(topic_root + "/odom", 0); + std::string odom_topic = "odom"; + std::string image_topic = "image"; + + // check backward compatibility with configs that have topic_root set + if (!topic_root.empty()) + { + image_topic = topic_root + "/camera/image"; + odom_topic = topic_root + "/odom"; + } + + // pubs + pub_vo = node.advertise(odom_topic, 0); + // subs image_transport::ImageTransport it(node); - image_transport::Subscriber sub = it.subscribe(topic_root + "/camera/image", 1, image_callback); + image_transport::Subscriber sub = it.subscribe(image_topic, 1, image_callback); ros::spin(); diff --git a/src/ratslam/experience_map.cpp b/src/ratslam/experience_map.cpp index da76932..2455aa7 100644 --- a/src/ratslam/experience_map.cpp +++ b/src/ratslam/experience_map.cpp @@ -36,7 +36,6 @@ using namespace std; namespace ratslam { - ExperienceMap::ExperienceMap(ptree settings) { get_setting_from_ptree(EXP_CORRECTION, settings, "exp_correction", 0.5); @@ -54,13 +53,12 @@ ExperienceMap::ExperienceMap(ptree settings) goal_timeout_s = 0; goal_success = false; - accum_delta_facing = EXP_INITIAL_EM_DEG * M_PI/180; + accum_delta_facing = EXP_INITIAL_EM_DEG * M_PI / 180; accum_delta_x = 0; accum_delta_y = 0; accum_delta_time_s = 0; relative_rad = 0; - } ExperienceMap::~ExperienceMap() @@ -69,12 +67,11 @@ ExperienceMap::~ExperienceMap() experiences.clear(); } -// create a new experience for a given position +// create a new experience for a given position int ExperienceMap::on_create_experience(unsigned int exp_id) { - experiences.resize(experiences.size() + 1); - Experience * new_exp = &(*(experiences.end() - 1)); + Experience* new_exp = &(*(experiences.end() - 1)); if (experiences.size() == 0) { @@ -118,8 +115,8 @@ bool ExperienceMap::iterate() int i; unsigned int link_id; unsigned int exp_id; - Experience * link_from, *link_to; - Link * link; + Experience* link_from, *link_to; + Link* link; double lx, ly, df; for (i = 0; i < EXP_LOOPS; i++) @@ -166,7 +163,7 @@ bool ExperienceMap::iterate() // create a link between two experiences bool ExperienceMap::on_create_link(int exp_id_from, int exp_id_to, double rel_rad) { - Experience * current_exp = &experiences[exp_id_from]; + Experience* current_exp = &experiences[exp_id_from]; // check if the link already exists for (unsigned int i = 0; i < experiences[exp_id_from].links_from.size(); i++) @@ -182,7 +179,7 @@ bool ExperienceMap::on_create_link(int exp_id_from, int exp_id_to, double rel_ra } links.resize(links.size() + 1); - Link * new_link = &(*(links.end() - 1)); + Link* new_link = &(*(links.end() - 1)); new_link->exp_to_id = exp_id_to; new_link->exp_from_id = exp_id_from; @@ -222,17 +219,16 @@ int ExperienceMap::on_set_experience(int new_exp_id, double rel_rad) struct compare { - bool operator()(const Experience *exp1, const Experience *exp2) + bool operator()(const Experience* exp1, const Experience* exp2) { return exp1->time_from_current_s > exp2->time_from_current_s; } }; -double exp_euclidean_m(Experience *exp1, Experience *exp2) +double exp_euclidean_m(Experience* exp1, Experience* exp2) { return sqrt( (double)((exp1->x_m - exp2->x_m) * (exp1->x_m - exp2->x_m) + (exp1->y_m - exp2->y_m) * (exp1->y_m - exp2->y_m))); - } double ExperienceMap::dijkstra_distance_between_experiences(int id1, int id2) @@ -265,7 +261,7 @@ double ExperienceMap::dijkstra_distance_between_experiences(int id1, int id2) for (id = 0; id < exp->links_to.size(); id++) { - Link *link = &links[exp->links_to[id]]; + Link* link = &links[exp->links_to[id]]; link_time_s = exp->time_from_current_s + link->delta_time_s; if (link_time_s < experiences[link->exp_from_id].time_from_current_s) { @@ -276,7 +272,7 @@ double ExperienceMap::dijkstra_distance_between_experiences(int id1, int id2) for (id = 0; id < exp->links_from.size(); id++) { - Link *link = &links[exp->links_from[id]]; + Link* link = &links[exp->links_from[id]]; link_time_s = exp->time_from_current_s + link->delta_time_s; if (link_time_s < experiences[link->exp_to_id].time_from_current_s) { @@ -298,7 +294,6 @@ double ExperienceMap::dijkstra_distance_between_experiences(int id1, int id2) // return true if path to goal found bool ExperienceMap::calculate_path_to_goal(double time_s) { - unsigned int id; waypoint_exp_id = -1; @@ -306,18 +301,18 @@ bool ExperienceMap::calculate_path_to_goal(double time_s) return false; // check if we are within thres of the goal or timeout - if (exp_euclidean_m(&experiences[current_exp_id], &experiences[goal_list[0]]) < 0.1 - || ((goal_timeout_s != 0) && time_s > goal_timeout_s)) + if (exp_euclidean_m(&experiences[current_exp_id], &experiences[goal_list[0]]) < 0.1 || + ((goal_timeout_s != 0) && time_s > goal_timeout_s)) { if (goal_timeout_s != 0 && time_s > goal_timeout_s) { -// cout << "Timed out reaching goal ... sigh" << endl; + // cout << "Timed out reaching goal ... sigh" << endl; goal_success = false; } if (exp_euclidean_m(&experiences[current_exp_id], &experiences[goal_list[0]]) < 0.1) { goal_success = true; - // cout << "Goal reached ... yay!" << endl; + // cout << "Goal reached ... yay!" << endl; } goal_list.pop_front(); goal_timeout_s = 0; @@ -362,7 +357,7 @@ bool ExperienceMap::calculate_path_to_goal(double time_s) for (id = 0; id < exp->links_to.size(); id++) { - Link *link = &links[exp->links_to[id]]; + Link* link = &links[exp->links_to[id]]; link_time_s = exp->time_from_current_s + link->delta_time_s; if (link_time_s < experiences[link->exp_from_id].time_from_current_s) { @@ -373,7 +368,7 @@ bool ExperienceMap::calculate_path_to_goal(double time_s) for (id = 0; id < exp->links_from.size(); id++) { - Link *link = &links[exp->links_from[id]]; + Link* link = &links[exp->links_from[id]]; link_time_s = exp->time_from_current_s + link->delta_time_s; if (link_time_s < experiences[link->exp_to_id].time_from_current_s) { @@ -385,7 +380,6 @@ bool ExperienceMap::calculate_path_to_goal(double time_s) if (!exp_heap.empty()) std::make_heap(const_cast(&exp_heap.top()), const_cast(&exp_heap.top()) + exp_heap.size(), compare()); - } // now do the current to goal links @@ -416,7 +410,7 @@ bool ExperienceMap::get_goal_waypoint() double dist; unsigned int trace_exp_id = goal_list[0]; - Experience *robot_exp = &experiences[current_exp_id]; + Experience* robot_exp = &experiences[current_exp_id]; while (trace_exp_id != goal_path_final_exp_id) { @@ -446,9 +440,8 @@ void ExperienceMap::add_goal(double x_m, double y_m) for (unsigned int i = 0; i < experiences.size(); i++) { - dist = sqrt( - (experiences[i].x_m - x_m) * (experiences[i].x_m - x_m) - + (experiences[i].y_m - y_m) * (experiences[i].y_m - y_m)); + dist = sqrt((experiences[i].x_m - x_m) * (experiences[i].x_m - x_m) + + (experiences[i].y_m - y_m) * (experiences[i].y_m - y_m)); if (dist < min_dist) { min_id = i; @@ -458,16 +451,14 @@ void ExperienceMap::add_goal(double x_m, double y_m) if (min_dist < 0.1) add_goal(min_id); - } double ExperienceMap::get_subgoal_m() const { - return ( - waypoint_exp_id == -1 ? 0 : - sqrt( - (double)pow((experiences[waypoint_exp_id].x_m - experiences[current_exp_id].x_m), 2) - + (double)pow((experiences[waypoint_exp_id].y_m - experiences[current_exp_id].y_m), 2))); + return (waypoint_exp_id == -1 ? + 0 : + sqrt((double)pow((experiences[waypoint_exp_id].x_m - experiences[current_exp_id].x_m), 2) + + (double)pow((experiences[waypoint_exp_id].y_m - experiences[current_exp_id].y_m), 2))); } double ExperienceMap::get_subgoal_rad() const @@ -482,5 +473,4 @@ double ExperienceMap::get_subgoal_rad() const } } -} // namespace ratslam - +} // namespace ratslam diff --git a/src/ratslam/experience_map.h b/src/ratslam/experience_map.h index 3391443..3d9f533 100644 --- a/src/ratslam/experience_map.h +++ b/src/ratslam/experience_map.h @@ -35,6 +35,7 @@ #define _EXPERIENCE_MAP_H_ #define _USE_MATH_DEFINES + #include "math.h" #include @@ -44,6 +45,7 @@ #include #include + using boost::property_tree::ptree; #include @@ -53,7 +55,6 @@ using boost::property_tree::ptree; namespace ratslam { - /* * The Link structure describes a link * between two experiences. @@ -68,17 +69,16 @@ struct Link int exp_from_id; double delta_time_s; - template - void serialize(Archive& ar, const unsigned int version) - { - ar & d; - ar & heading_rad; - ar & facing_rad; - ar & exp_to_id; - ar & exp_from_id; - ar & delta_time_s; - } - + template + void serialize(Archive &ar, const unsigned int version) + { + ar &d; + ar &heading_rad; + ar &facing_rad; + ar &exp_to_id; + ar &exp_from_id; + ar &delta_time_s; + } }; /* @@ -87,52 +87,52 @@ struct Link */ struct Experience { - int id; // its own id + int id; // its own id double x_m, y_m, th_rad; int vt_id; - std::vector links_from; // links from this experience - std::vector links_to; // links to this experience - + std::vector links_from; // links from this experience + std::vector links_to; // links to this experience // goal navigation double time_from_current_s; unsigned int goal_to_current, current_to_goal; - template - void serialize(Archive& ar, const unsigned int version) - { - ar & id; - ar & x_m & y_m & th_rad; - ar & vt_id; - ar & links_from & links_to; - ar & time_from_current_s; - ar & goal_to_current & current_to_goal; - } - + template + void serialize(Archive &ar, const unsigned int version) + { + ar &id; + ar &x_m &y_m &th_rad; + ar &vt_id; + ar &links_from &links_to; + ar &time_from_current_s; + ar &goal_to_current ¤t_to_goal; + } }; class ExperienceMapScene; class ExperienceMap { - public: friend class ExperienceMapScene; ExperienceMap(ptree settings); + ~ExperienceMap(); // create a new experience for a given position int on_create_experience(unsigned int exp_id); + bool on_create_link(int exp_id_from, int exp_id_to, double rel_rad); Experience *get_experience(int id) { return &experiences[id]; } - Link * get_link(int id) + + Link *get_link(int id) { return &links[id]; } @@ -164,29 +164,38 @@ class ExperienceMap // functions for setting and handling goals. void add_goal(double x_m, double y_m); + void add_goal(int id) { goal_list.push_back(id); } + bool calculate_path_to_goal(double time_s); + bool get_goal_waypoint(); + void clear_goal_list() { goal_list.clear(); } + int get_current_goal_id() { return (goal_list.size() == 0) ? -1 : (int)goal_list.front(); } + void delete_current_goal() { goal_list.pop_front(); } + bool get_goal_success() { return goal_success; } + double get_subgoal_m() const; + double get_subgoal_rad() const; const std::deque &get_goals() const @@ -196,37 +205,35 @@ class ExperienceMap unsigned int get_goal_path_final_exp() { - return goal_path_final_exp_id; + return goal_path_final_exp_id; } + template + void serialize(Archive &ar, const unsigned int version) + { + ar &EXP_LOOPS; + ar &EXP_CORRECTION; + ar &MAX_GOALS; + ar &EXP_INITIAL_EM_DEG; - template - void serialize(Archive& ar, const unsigned int version) - { - ar & EXP_LOOPS; - ar & EXP_CORRECTION; - ar & MAX_GOALS; - ar & EXP_INITIAL_EM_DEG; - - ar & experiences; - ar & links; - ar & goal_list; + ar &experiences; + ar &links; + ar &goal_list; - ar & current_exp_id & prev_exp_id; + ar ¤t_exp_id &prev_exp_id; - ar & accum_delta_facing; - ar & accum_delta_x; - ar & accum_delta_y; - ar & accum_delta_time_s; + ar &accum_delta_facing; + ar &accum_delta_x; + ar &accum_delta_y; + ar &accum_delta_time_s; - ar & waypoint_exp_id; - ar & goal_success; - ar & goal_timeout_s; - ar & goal_path_final_exp_id; - - ar & relative_rad; + ar &waypoint_exp_id; + ar &goal_success; + ar &goal_timeout_s; + ar &goal_path_final_exp_id; - } + ar &relative_rad; + } private: friend class boost::serialization::access; @@ -235,11 +242,11 @@ class ExperienceMap { ; } + // calculate distance between two experiences using djikstras algorithm // can be very slow for many experiences double dijkstra_distance_between_experiences(int id1, int id2); - int EXP_LOOPS; double EXP_CORRECTION; unsigned int MAX_GOALS; @@ -262,9 +269,7 @@ class ExperienceMap bool goal_success; double goal_timeout_s; unsigned int goal_path_final_exp_id; - }; - } -#endif // _EXPERIENCE_MAP_H_ +#endif // _EXPERIENCE_MAP_H_ diff --git a/src/ratslam/local_view_match.cpp b/src/ratslam/local_view_match.cpp index 5e4b5af..f543d59 100644 --- a/src/ratslam/local_view_match.cpp +++ b/src/ratslam/local_view_match.cpp @@ -34,7 +34,9 @@ #include #include #include + using namespace std; + #include #include @@ -42,18 +44,15 @@ using namespace std; namespace ratslam { - - - LocalViewMatch::LocalViewMatch(ptree settings) { get_setting_from_ptree(VT_MIN_PATCH_NORMALISATION_STD, settings, "vt_min_patch_normalisation_std", (double)0); get_setting_from_ptree(VT_PATCH_NORMALISATION, settings, "vt_patch_normalise", 0); - get_setting_from_ptree(VT_NORMALISATION, settings, "vt_normalisation", (double) 0); + get_setting_from_ptree(VT_NORMALISATION, settings, "vt_normalisation", (double)0); get_setting_from_ptree(VT_SHIFT_MATCH, settings, "vt_shift_match", 25); get_setting_from_ptree(VT_STEP_MATCH, settings, "vt_step_match", 5); get_setting_from_ptree(VT_PANORAMIC, settings, "vt_panoramic", 0); - + get_setting_from_ptree(VT_MATCH_THRESHOLD, settings, "vt_match_threshold", 0.03); get_setting_from_ptree(TEMPLATE_X_SIZE, settings, "template_x_size", 1); get_setting_from_ptree(TEMPLATE_Y_SIZE, settings, "template_y_size", 1); @@ -72,13 +71,12 @@ LocalViewMatch::LocalViewMatch(ptree settings) prev_vt = 0; } - LocalViewMatch::~LocalViewMatch() { - } -void LocalViewMatch::on_image(const unsigned char *view_rgb, bool greyscale, unsigned int image_width, unsigned int image_height) +void LocalViewMatch::on_image(const unsigned char* view_rgb, bool greyscale, unsigned int image_width, + unsigned int image_height) { if (view_rgb == NULL) return; @@ -111,11 +109,9 @@ void LocalViewMatch::on_image(const unsigned char *view_rgb, bool greyscale, uns cout << "VTN[" << setw(4) << get_current_vt() << "] " << endl; cout.flush(); } - } - -void LocalViewMatch::clip_view_x_y(int &x, int &y) +void LocalViewMatch::clip_view_x_y(int& x, int& y) { if (x < 0) x = 0; @@ -126,7 +122,6 @@ void LocalViewMatch::clip_view_x_y(int &x, int &y) y = 0; else if (y > TEMPLATE_Y_SIZE - 1) y = TEMPLATE_Y_SIZE - 1; - } void LocalViewMatch::convert_view_to_view_template(bool grayscale) @@ -143,11 +138,11 @@ void LocalViewMatch::convert_view_to_view_template(bool grayscale) if (grayscale) { - for (int y_block = IMAGE_VT_Y_RANGE_MIN, y_block_count = 0; y_block_count < TEMPLATE_Y_SIZE; y_block += - y_block_size, y_block_count++) + for (int y_block = IMAGE_VT_Y_RANGE_MIN, y_block_count = 0; y_block_count < TEMPLATE_Y_SIZE; + y_block += y_block_size, y_block_count++) { - for (int x_block = IMAGE_VT_X_RANGE_MIN, x_block_count = 0; x_block_count < TEMPLATE_X_SIZE; x_block += - x_block_size, x_block_count++) + for (int x_block = IMAGE_VT_X_RANGE_MIN, x_block_count = 0; x_block_count < TEMPLATE_X_SIZE; + x_block += x_block_size, x_block_count++) { for (int x = x_block; x < (x_block + x_block_size); x++) { @@ -165,19 +160,19 @@ void LocalViewMatch::convert_view_to_view_template(bool grayscale) } else { - for (int y_block = IMAGE_VT_Y_RANGE_MIN, y_block_count = 0; y_block_count < TEMPLATE_Y_SIZE; y_block += - y_block_size, y_block_count++) + for (int y_block = IMAGE_VT_Y_RANGE_MIN, y_block_count = 0; y_block_count < TEMPLATE_Y_SIZE; + y_block += y_block_size, y_block_count++) { - for (int x_block = IMAGE_VT_X_RANGE_MIN, x_block_count = 0; x_block_count < TEMPLATE_X_SIZE; x_block += - x_block_size, x_block_count++) + for (int x_block = IMAGE_VT_X_RANGE_MIN, x_block_count = 0; x_block_count < TEMPLATE_X_SIZE; + x_block += x_block_size, x_block_count++) { for (int x = x_block; x < (x_block + x_block_size); x++) { for (int y = y_block; y < (y_block + y_block_size); y++) { pos = (x + y * IMAGE_WIDTH) * 3; - current_view[data_next] += ((double)(view_rgb[pos]) + (double)(view_rgb[pos + 1]) - + (double)(view_rgb[pos + 2])); + current_view[data_next] += + ((double)(view_rgb[pos]) + (double)(view_rgb[pos + 1]) + (double)(view_rgb[pos + 2])); } } current_view[data_next] /= (255.0 * 3.0); @@ -251,8 +246,8 @@ void LocalViewMatch::convert_view_to_view_template(bool grayscale) patch_y_clip = patch_y; clip_view_x_y(patch_x_clip, patch_y_clip); - patch_sum += ((current_view_copy[patch_x_clip + patch_y_clip * TEMPLATE_X_SIZE] - patch_mean) - * (current_view_copy[patch_x_clip + patch_y_clip * TEMPLATE_X_SIZE] - patch_mean)); + patch_sum += ((current_view_copy[patch_x_clip + patch_y_clip * TEMPLATE_X_SIZE] - patch_mean) * + (current_view_copy[patch_x_clip + patch_y_clip * TEMPLATE_X_SIZE] - patch_mean)); } } @@ -260,8 +255,11 @@ void LocalViewMatch::convert_view_to_view_template(bool grayscale) if (patch_std < VT_MIN_PATCH_NORMALISATION_STD) current_view[x + y * TEMPLATE_X_SIZE] = 0.5; - else { - current_view[x + y * TEMPLATE_X_SIZE] = max((double) 0, min(1.0, (((current_view_copy[x + y * TEMPLATE_X_SIZE] - patch_mean) / patch_std) + 3.0)/6.0 )); + else + { + current_view[x + y * TEMPLATE_X_SIZE] = + max((double)0, + min(1.0, (((current_view_copy[x + y * TEMPLATE_X_SIZE] - patch_mean) / patch_std) + 3.0) / 6.0)); } } } @@ -273,18 +271,17 @@ void LocalViewMatch::convert_view_to_view_template(bool grayscale) for (int i = 0; i < current_view.size(); i++) sum += current_view[i]; - current_mean = sum/current_view.size(); - + current_mean = sum / current_view.size(); } // create and add a visual template to the collection int LocalViewMatch::create_template() { templates.resize(templates.size() + 1); - VisualTemplate * new_template = &(*(templates.end() - 1)); + VisualTemplate* new_template = &(*(templates.end() - 1)); new_template->id = templates.size() - 1; - double * data_ptr = ¤t_view[0]; + double* data_ptr = ¤t_view[0]; new_template->data.reserve(TEMPLATE_SIZE); for (int i = 0; i < TEMPLATE_SIZE; i++) new_template->data.push_back(*(data_ptr++)); @@ -294,10 +291,10 @@ int LocalViewMatch::create_template() return templates.size() - 1; } -// compare a visual template to all the stored templates, allowing for +// compare a visual template to all the stored templates, allowing for // slen pixel shifts in each direction // returns the matching template and the MSE -void LocalViewMatch::compare(double &vt_err, unsigned int &vt_match_id) +void LocalViewMatch::compare(double& vt_err, unsigned int& vt_match_id) { if (templates.size() == 0) { @@ -306,22 +303,22 @@ void LocalViewMatch::compare(double &vt_err, unsigned int &vt_match_id) return; } - double *data = ¤t_view[0]; + double* data = ¤t_view[0]; double mindiff, cdiff; mindiff = DBL_MAX; vt_err = DBL_MAX; int min_template = 0; - double *template_ptr; - double *column_ptr; - double *template_row_ptr; - double *column_row_ptr; - double *template_start_ptr; - double *column_start_ptr; + double* template_ptr; + double* column_ptr; + double* template_row_ptr; + double* column_row_ptr; + double* template_start_ptr; + double* column_start_ptr; int row_size; int sub_row_size; - double *column_end_ptr; + double* column_end_ptr; VisualTemplate vt; int min_offset; @@ -330,126 +327,124 @@ void LocalViewMatch::compare(double &vt_err, unsigned int &vt_match_id) if (VT_PANORAMIC) { + BOOST_FOREACH (vt, templates) + { + if (abs(current_mean - vt.mean) > VT_MATCH_THRESHOLD + epsilon) + continue; + + // for each vt try matching the view at different offsets + // try to fast break based on error already great than previous errors + // handles 2d images shifting only in the x direction + // note I haven't tested on a 1d yet. + for (offset = 0; offset < TEMPLATE_X_SIZE; offset += VT_STEP_MATCH) + { + cdiff = 0; + template_start_ptr = &vt.data[0] + offset; + column_start_ptr = &data[0]; + row_size = TEMPLATE_X_SIZE; + column_end_ptr = &data[0] + TEMPLATE_SIZE - offset; + sub_row_size = TEMPLATE_X_SIZE - offset; + + // do from offset to end + for (column_row_ptr = column_start_ptr, template_row_ptr = template_start_ptr; column_row_ptr < column_end_ptr; + column_row_ptr += row_size, template_row_ptr += row_size) + { + for (column_ptr = column_row_ptr, template_ptr = template_row_ptr; column_ptr < column_row_ptr + sub_row_size; + column_ptr++, template_ptr++) + { + cdiff += abs(*column_ptr - *template_ptr); + } + + // fast breaks + if (cdiff > mindiff) + break; + } + + // do from start to offset + template_start_ptr = &vt.data[0]; + column_start_ptr = &data[0] + TEMPLATE_X_SIZE - offset; + row_size = TEMPLATE_X_SIZE; + column_end_ptr = &data[0] + TEMPLATE_SIZE; + sub_row_size = offset; + for (column_row_ptr = column_start_ptr, template_row_ptr = template_start_ptr; column_row_ptr < column_end_ptr; + column_row_ptr += row_size, template_row_ptr += row_size) + { + for (column_ptr = column_row_ptr, template_ptr = template_row_ptr; column_ptr < column_row_ptr + sub_row_size; + column_ptr++, template_ptr++) + { + cdiff += abs(*column_ptr - *template_ptr); + } + + // fast breaks + if (cdiff > mindiff) + break; + } + + if (cdiff < mindiff) + { + mindiff = cdiff; + min_template = vt.id; + min_offset = offset; + } + } + } + + vt_relative_rad = (double)min_offset / TEMPLATE_X_SIZE * 2.0 * M_PI; + if (vt_relative_rad > M_PI) + vt_relative_rad = vt_relative_rad - 2.0 * M_PI; + vt_err = mindiff / (double)TEMPLATE_SIZE; + vt_match_id = min_template; + + vt_error = vt_err; + } + else + { + BOOST_FOREACH (vt, templates) + { + if (abs(current_mean - vt.mean) > VT_MATCH_THRESHOLD + epsilon) + continue; + + // for each vt try matching the view at different offsets + // try to fast break based on error already great than previous errors + // handles 2d images shifting only in the x direction + // note I haven't tested on a 1d yet. + for (offset = 0; offset < VT_SHIFT_MATCH * 2 + 1; offset += VT_STEP_MATCH) + { + cdiff = 0; + template_start_ptr = &vt.data[0] + offset; + column_start_ptr = &data[0] + VT_SHIFT_MATCH; + row_size = TEMPLATE_X_SIZE; + column_end_ptr = &data[0] + TEMPLATE_SIZE - VT_SHIFT_MATCH; + sub_row_size = TEMPLATE_X_SIZE - 2 * VT_SHIFT_MATCH; + + for (column_row_ptr = column_start_ptr, template_row_ptr = template_start_ptr; column_row_ptr < column_end_ptr; + column_row_ptr += row_size, template_row_ptr += row_size) + { + for (column_ptr = column_row_ptr, template_ptr = template_row_ptr; column_ptr < column_row_ptr + sub_row_size; + column_ptr++, template_ptr++) + { + cdiff += abs(*column_ptr - *template_ptr); + } - BOOST_FOREACH(vt, templates) - { - - if (abs(current_mean - vt.mean) > VT_MATCH_THRESHOLD + epsilon) - continue; - - // for each vt try matching the view at different offsets - // try to fast break based on error already great than previous errors - // handles 2d images shifting only in the x direction - // note I haven't tested on a 1d yet. - for (offset = 0; offset < TEMPLATE_X_SIZE; offset += VT_STEP_MATCH) - { - cdiff = 0; - template_start_ptr = &vt.data[0] + offset; - column_start_ptr = &data[0]; - row_size = TEMPLATE_X_SIZE; - column_end_ptr = &data[0] + TEMPLATE_SIZE - offset; - sub_row_size = TEMPLATE_X_SIZE - offset; - - // do from offset to end - for (column_row_ptr = column_start_ptr, template_row_ptr = template_start_ptr; column_row_ptr < column_end_ptr; column_row_ptr+=row_size, template_row_ptr+=row_size) - { - for (column_ptr = column_row_ptr, template_ptr = template_row_ptr; column_ptr < column_row_ptr + sub_row_size; column_ptr++, template_ptr++) - { - cdiff += abs(*column_ptr - *template_ptr); - } - - // fast breaks - if (cdiff > mindiff) - break; - } - - // do from start to offset - template_start_ptr = &vt.data[0]; - column_start_ptr = &data[0] + TEMPLATE_X_SIZE - offset; - row_size = TEMPLATE_X_SIZE; - column_end_ptr = &data[0] + TEMPLATE_SIZE; - sub_row_size = offset; - for (column_row_ptr = column_start_ptr, template_row_ptr = template_start_ptr; column_row_ptr < column_end_ptr; column_row_ptr+=row_size, template_row_ptr+=row_size) - { - for (column_ptr = column_row_ptr, template_ptr = template_row_ptr; column_ptr < column_row_ptr + sub_row_size; column_ptr++, template_ptr++) - { - cdiff += abs(*column_ptr - *template_ptr); - } - - // fast breaks - if (cdiff > mindiff) - break; - } - - - if (cdiff < mindiff) - { - mindiff = cdiff; - min_template = vt.id; - min_offset = offset; - } - } - - } - - vt_relative_rad = (double) min_offset/TEMPLATE_X_SIZE * 2.0 * M_PI; - if (vt_relative_rad > M_PI) - vt_relative_rad = vt_relative_rad - 2.0 * M_PI; - vt_err = mindiff / (double) TEMPLATE_SIZE; - vt_match_id = min_template; - - vt_error = vt_err; - - } else { - - BOOST_FOREACH(vt, templates) - { - - if (abs(current_mean - vt.mean) > VT_MATCH_THRESHOLD + epsilon) - continue; - - // for each vt try matching the view at different offsets - // try to fast break based on error already great than previous errors - // handles 2d images shifting only in the x direction - // note I haven't tested on a 1d yet. - for (offset = 0; offset < VT_SHIFT_MATCH*2+1; offset += VT_STEP_MATCH) - { - cdiff = 0; - template_start_ptr = &vt.data[0] + offset; - column_start_ptr = &data[0] + VT_SHIFT_MATCH; - row_size = TEMPLATE_X_SIZE; - column_end_ptr = &data[0] + TEMPLATE_SIZE - VT_SHIFT_MATCH; - sub_row_size = TEMPLATE_X_SIZE - 2*VT_SHIFT_MATCH; - - for (column_row_ptr = column_start_ptr, template_row_ptr = template_start_ptr; column_row_ptr < column_end_ptr; column_row_ptr+=row_size, template_row_ptr+=row_size) - { - for (column_ptr = column_row_ptr, template_ptr = template_row_ptr; column_ptr < column_row_ptr + sub_row_size; column_ptr++, template_ptr++) - { - cdiff += abs(*column_ptr - *template_ptr); - } - - // fast breaks - if (cdiff > mindiff) - break; - } - - if (cdiff < mindiff) - { - mindiff = cdiff; - min_template = vt.id; - min_offset = 0; - } - } - - } - - vt_relative_rad = 0; - vt_err = mindiff / (double)(TEMPLATE_SIZE - 2 * VT_SHIFT_MATCH * TEMPLATE_Y_SIZE); - vt_match_id = min_template; - - vt_error = vt_err; + // fast breaks + if (cdiff > mindiff) + break; + } + if (cdiff < mindiff) + { + mindiff = cdiff; + min_template = vt.id; + min_offset = 0; + } + } + } + + vt_relative_rad = 0; + vt_err = mindiff / (double)(TEMPLATE_SIZE - 2 * VT_SHIFT_MATCH * TEMPLATE_Y_SIZE); + vt_match_id = min_template; + + vt_error = vt_err; } } - } diff --git a/src/ratslam/local_view_match.h b/src/ratslam/local_view_match.h index c8cabed..8f2d635 100644 --- a/src/ratslam/local_view_match.h +++ b/src/ratslam/local_view_match.h @@ -43,9 +43,11 @@ #include #define _USE_MATH_DEFINES + #include "math.h" #include + using boost::property_tree::ptree; #include @@ -54,21 +56,19 @@ using boost::property_tree::ptree; namespace ratslam { - struct VisualTemplate { unsigned int id; std::vector data; double mean; - template - void serialize(Archive& ar, const unsigned int version) - { - ar & id; - ar & data; - ar & mean; - } - + template + void serialize(Archive &ar, const unsigned int version) + { + ar &id; + ar &data; + ar &mean; + } }; class LocalViewMatch @@ -92,36 +92,35 @@ class LocalViewMatch return vt_relative_rad; } - template - void serialize(Archive& ar, const unsigned int version) - { - ar & VT_SHIFT_MATCH; - ar & VT_STEP_MATCH; - ar & VT_MATCH_THRESHOLD; - ar & TEMPLATE_SIZE; - ar & IMAGE_WIDTH; - ar & IMAGE_HEIGHT; - ar & IMAGE_VT_X_RANGE_MIN; - ar & IMAGE_VT_X_RANGE_MAX; - ar & IMAGE_VT_Y_RANGE_MIN; - ar & IMAGE_VT_Y_RANGE_MAX; - ar & TEMPLATE_X_SIZE; - ar & TEMPLATE_Y_SIZE; - ar & VT_PATCH_NORMALISATION; - ar & VT_MIN_PATCH_NORMALISATION_STD; - ar & VT_NORMALISATION; - ar & VT_PANORAMIC; - - ar & templates; - ar & current_view; - ar & current_mean; - ar & image_size; - ar & current_vt; - ar & vt_error; - ar & prev_vt; - ar & vt_relative_rad; - - } + template + void serialize(Archive &ar, const unsigned int version) + { + ar &VT_SHIFT_MATCH; + ar &VT_STEP_MATCH; + ar &VT_MATCH_THRESHOLD; + ar &TEMPLATE_SIZE; + ar &IMAGE_WIDTH; + ar &IMAGE_HEIGHT; + ar &IMAGE_VT_X_RANGE_MIN; + ar &IMAGE_VT_X_RANGE_MAX; + ar &IMAGE_VT_Y_RANGE_MIN; + ar &IMAGE_VT_Y_RANGE_MAX; + ar &TEMPLATE_X_SIZE; + ar &TEMPLATE_Y_SIZE; + ar &VT_PATCH_NORMALISATION; + ar &VT_MIN_PATCH_NORMALISATION_STD; + ar &VT_NORMALISATION; + ar &VT_PANORAMIC; + + ar &templates; + ar ¤t_view; + ar ¤t_mean; + ar &image_size; + ar ¤t_vt; + ar &vt_error; + ar &prev_vt; + ar &vt_relative_rad; + } private: friend class boost::serialization::access; @@ -130,6 +129,7 @@ class LocalViewMatch { ; } + void clip_view_x_y(int &x, int &y); // create and add a visual template to the collection @@ -180,9 +180,8 @@ class LocalViewMatch const unsigned char *view_rgb; bool greyscale; - }; -} // namespace ratslam +} // namespace ratslam -#endif // _VISUAL_TEMPLATE_MATCH_H_ +#endif // _VISUAL_TEMPLATE_MATCH_H_ diff --git a/src/ratslam/posecell_network.cpp b/src/ratslam/posecell_network.cpp index d128ef9..658f119 100644 --- a/src/ratslam/posecell_network.cpp +++ b/src/ratslam/posecell_network.cpp @@ -38,7 +38,6 @@ using namespace std; namespace ratslam { - PosecellNetwork::PosecellNetwork(ptree settings) { /* @@ -71,7 +70,6 @@ PosecellNetwork::PosecellNetwork(ptree settings) odo_update = false; vt_update = false; - } void PosecellNetwork::pose_cell_builder() @@ -85,22 +83,22 @@ void PosecellNetwork::pose_cell_builder() posecells_elements = PC_DIM_XY * PC_DIM_XY * PC_DIM_TH; // allocate the memory - posecells_memory = (Posecell *)malloc((size_t)posecells_memory_size); - pca_new_memory = (Posecell *)malloc((size_t)posecells_memory_size); + posecells_memory = (Posecell*)malloc((size_t)posecells_memory_size); + pca_new_memory = (Posecell*)malloc((size_t)posecells_memory_size); // zero all the memory memset(posecells_memory, 0, (size_t)posecells_memory_size); memset(pca_new_memory, 0, (size_t)posecells_memory_size); // allocate first level pointers - posecells = (Posecell ***)malloc(sizeof(Posecell**) * PC_DIM_TH); - pca_new = (Posecell ***)malloc(sizeof(Posecell**) * PC_DIM_TH); + posecells = (Posecell***)malloc(sizeof(Posecell**) * PC_DIM_TH); + pca_new = (Posecell***)malloc(sizeof(Posecell**) * PC_DIM_TH); for (i = 0; i < PC_DIM_TH; i++) { // allocate second level pointers - posecells[i] = (Posecell **)malloc(sizeof(Posecell*) * PC_DIM_XY); - pca_new[i] = (Posecell **)malloc(sizeof(Posecell*) * PC_DIM_XY); + posecells[i] = (Posecell**)malloc(sizeof(Posecell*) * PC_DIM_XY); + pca_new[i] = (Posecell**)malloc(sizeof(Posecell*) * PC_DIM_XY); for (j = 0; j < PC_DIM_XY; j++) { @@ -111,18 +109,18 @@ void PosecellNetwork::pose_cell_builder() } // for path integration - pca_new_rot_ptr = (Posecell **)malloc(sizeof(Posecell*) * (PC_DIM_XY + 2)); - pca_new_rot_ptr2 = (Posecell **)malloc(sizeof(Posecell*) * (PC_DIM_XY + 2)); + pca_new_rot_ptr = (Posecell**)malloc(sizeof(Posecell*) * (PC_DIM_XY + 2)); + pca_new_rot_ptr2 = (Posecell**)malloc(sizeof(Posecell*) * (PC_DIM_XY + 2)); for (i = 0; i < PC_DIM_XY + 2; i++) { pca_new_rot_ptr[i] = &pca_new_memory[(i * (PC_DIM_XY + 2))]; pca_new_rot_ptr2[i] = &pca_new_memory[(PC_DIM_XY + 2) * (PC_DIM_XY + 2) + (i * (PC_DIM_XY + 2))]; } - posecells_plane_th = (Posecell *)malloc(sizeof(Posecell) * (PC_DIM_XY + 2) * (PC_DIM_XY + 2)); + posecells_plane_th = (Posecell*)malloc(sizeof(Posecell) * (PC_DIM_XY + 2) * (PC_DIM_XY + 2)); - PC_W_EXCITE = (double *)malloc(sizeof(double) * PC_W_E_DIM * PC_W_E_DIM * PC_W_E_DIM); - PC_W_INHIB = (double *)malloc(sizeof(double) * PC_W_I_DIM * PC_W_I_DIM * PC_W_I_DIM); + PC_W_EXCITE = (double*)malloc(sizeof(double) * PC_W_E_DIM * PC_W_E_DIM * PC_W_E_DIM); + PC_W_INHIB = (double*)malloc(sizeof(double) * PC_W_I_DIM * PC_W_I_DIM * PC_W_I_DIM); posecells[(int)best_th][(int)best_y][(int)best_x] = 1; @@ -130,10 +128,10 @@ void PosecellNetwork::pose_cell_builder() PC_W_E_DIM_HALF = (int)floor((double)PC_W_E_DIM / 2.0); PC_W_I_DIM_HALF = (int)floor((double)PC_W_I_DIM / 2.0); - PC_E_XY_WRAP = (int *)malloc((PC_DIM_XY + PC_W_E_DIM - 1) * sizeof(int)); - PC_E_TH_WRAP = (int *)malloc((PC_DIM_TH + PC_W_E_DIM - 1) * sizeof(int)); - PC_I_XY_WRAP = (int *)malloc((PC_DIM_XY + PC_W_I_DIM - 1) * sizeof(int)); - PC_I_TH_WRAP = (int *)malloc((PC_DIM_TH + PC_W_I_DIM - 1) * sizeof(int)); + PC_E_XY_WRAP = (int*)malloc((PC_DIM_XY + PC_W_E_DIM - 1) * sizeof(int)); + PC_E_TH_WRAP = (int*)malloc((PC_DIM_TH + PC_W_E_DIM - 1) * sizeof(int)); + PC_I_XY_WRAP = (int*)malloc((PC_DIM_XY + PC_W_I_DIM - 1) * sizeof(int)); + PC_I_TH_WRAP = (int*)malloc((PC_DIM_TH + PC_W_I_DIM - 1) * sizeof(int)); generate_wrap(PC_E_XY_WRAP, PC_DIM_XY - PC_W_E_DIM_HALF, PC_DIM_XY, 0, PC_DIM_XY, 0, PC_W_E_DIM_HALF); generate_wrap(PC_E_TH_WRAP, PC_DIM_TH - PC_W_E_DIM_HALF, PC_DIM_TH, 0, PC_DIM_TH, 0, PC_W_E_DIM_HALF); @@ -141,17 +139,17 @@ void PosecellNetwork::pose_cell_builder() generate_wrap(PC_I_TH_WRAP, PC_DIM_TH - PC_W_I_DIM_HALF, PC_DIM_TH, 0, PC_DIM_TH, 0, PC_W_I_DIM_HALF); PC_CELLS_TO_AVG = 3; - PC_AVG_XY_WRAP = (int *)malloc((PC_DIM_XY + 2 * PC_CELLS_TO_AVG) * sizeof(int)); - PC_AVG_TH_WRAP = (int *)malloc((PC_DIM_TH + 2 * PC_CELLS_TO_AVG) * sizeof(int)); + PC_AVG_XY_WRAP = (int*)malloc((PC_DIM_XY + 2 * PC_CELLS_TO_AVG) * sizeof(int)); + PC_AVG_TH_WRAP = (int*)malloc((PC_DIM_TH + 2 * PC_CELLS_TO_AVG) * sizeof(int)); generate_wrap(PC_AVG_XY_WRAP, PC_DIM_XY - PC_CELLS_TO_AVG, PC_DIM_XY, 0, PC_DIM_XY, 0, PC_CELLS_TO_AVG); generate_wrap(PC_AVG_TH_WRAP, PC_DIM_TH - PC_CELLS_TO_AVG, PC_DIM_TH, 0, PC_DIM_TH, 0, PC_CELLS_TO_AVG); // sine and cosine lookups - PC_XY_SUM_SIN_LOOKUP = (double *)malloc(PC_DIM_XY * sizeof(double)); - PC_XY_SUM_COS_LOOKUP = (double *)malloc(PC_DIM_XY * sizeof(double)); - PC_TH_SUM_SIN_LOOKUP = (double *)malloc(PC_DIM_TH * sizeof(double)); - PC_TH_SUM_COS_LOOKUP = (double *)malloc(PC_DIM_TH * sizeof(double)); + PC_XY_SUM_SIN_LOOKUP = (double*)malloc(PC_DIM_XY * sizeof(double)); + PC_XY_SUM_COS_LOOKUP = (double*)malloc(PC_DIM_XY * sizeof(double)); + PC_TH_SUM_SIN_LOOKUP = (double*)malloc(PC_DIM_TH * sizeof(double)); + PC_TH_SUM_COS_LOOKUP = (double*)malloc(PC_DIM_TH * sizeof(double)); for (i = 0; i < PC_DIM_XY; i++) { @@ -239,7 +237,6 @@ PosecellNetwork::~PosecellNetwork() bool PosecellNetwork::inject(int act_x, int act_y, int act_z, double energy) { - if (act_x < PC_DIM_XY && act_x >= 0 && act_y < PC_DIM_XY && act_y >= 0 && act_z < PC_DIM_TH && act_z >= 0) posecells[act_z][act_y][act_x] += energy; @@ -272,7 +269,6 @@ bool PosecellNetwork::excite(void) // pc.Posecells = pca_new; memcpy(posecells_memory, pca_new_memory, posecells_memory_size); return true; - } bool PosecellNetwork::inhibit(void) @@ -338,12 +334,11 @@ bool PosecellNetwork::normalise(void) for (i = 0; i < posecells_elements; i++) { posecells_memory[i] /= total; - //assert(posecells_memory[i] >= 0); - //assert(!isnan(posecells_memory[i])); + // assert(posecells_memory[i] >= 0); + // assert(!isnan(posecells_memory[i])); } return true; - } bool PosecellNetwork::path_integration(double vtrans, double vrot) @@ -363,7 +358,6 @@ bool PosecellNetwork::path_integration(double vtrans, double vrot) // for dir_pc=1:PARAMS.PC_DIM_TH for (dir_pc = 0; dir_pc < PC_DIM_TH; dir_pc++) { - // % radians // dir = (dir_pc - 1) * pc.PC_C_SIZE_TH; double dir = dir_pc * PC_C_SIZE_TH + angle_to_add; @@ -400,11 +394,11 @@ bool PosecellNetwork::path_integration(double vtrans, double vrot) weight_sw = vtrans * vtrans * cos(dir90) * sin(dir90); // weight_se = vtrans*sin(dir90) - vtrans^2 *cos(dir90) * sin(dir90); - weight_se = //vtrans*sin(dir90) - vtrans*vtrans*cos(dir90)*sin(dir90); + weight_se = // vtrans*sin(dir90) - vtrans*vtrans*cos(dir90)*sin(dir90); vtrans * sin(dir90) * (1.0 - vtrans * cos(dir90)); // weight_nw = vtrans*cos(dir90) - vtrans^2 *cos(dir90) * sin(dir90); - weight_nw = // vtrans*cos(dir90) - vtrans*vtrans*cos(dir90)*sin(dir90); + weight_nw = // vtrans*cos(dir90) - vtrans*vtrans*cos(dir90)*sin(dir90); vtrans * cos(dir90) * (1.0 - vtrans * sin(dir90)); // weight_ne = 1.0 - weight_sw - weight_se - weight_nw; @@ -413,30 +407,36 @@ bool PosecellNetwork::path_integration(double vtrans, double vrot) /* if (weight_sw < 0 || weight_se < 0 || weight_nw < 0 || weight_ne < 0) { printf("WARNING: weights are negative, vtrans(%f) is either negative or too big\n", vtrans); - printf("WARNING: continuing, but expect possible failures soon! Update POSECELL_VTRANS_SCALING to fix this.\n", vtrans); + printf("WARNING: continuing, but expect possible failures soon! Update POSECELL_VTRANS_SCALING to fix this.\n", + vtrans); }*/ // % circular shift and multiple by the contributing weight // % copy those shifted elements for the wrap around - // pca_new = pca_new.*weight_ne + [pca_new(:,end) pca_new(:,1:end-1)].*weight_nw + [pca_new(end,:); pca_new(1:end-1,:)].*weight_se + circshift(pca_new, [1 1]).*weight_sw; + // pca_new = pca_new.*weight_ne + [pca_new(:,end) pca_new(:,1:end-1)].*weight_nw + [pca_new(end,:); + // pca_new(1:end-1,:)].*weight_se + circshift(pca_new, [1 1]).*weight_sw; // first element - pca_new_rot_ptr2[0][0] = pca_new_rot_ptr[0][0] * weight_ne + pca_new_rot_ptr[0][PC_DIM_XY + 1] * weight_se + pca_new_rot_ptr[PC_DIM_XY + 1][0] * weight_nw; + pca_new_rot_ptr2[0][0] = pca_new_rot_ptr[0][0] * weight_ne + pca_new_rot_ptr[0][PC_DIM_XY + 1] * weight_se + + pca_new_rot_ptr[PC_DIM_XY + 1][0] * weight_nw; // first row for (i = 1; i < PC_DIM_XY + 2; i++) { - pca_new_rot_ptr2[0][i] = pca_new_rot_ptr[0][i] * weight_ne + pca_new_rot_ptr[0][i - 1] * weight_se + pca_new_rot_ptr[PC_DIM_XY + 1][i] * weight_nw; + pca_new_rot_ptr2[0][i] = pca_new_rot_ptr[0][i] * weight_ne + pca_new_rot_ptr[0][i - 1] * weight_se + + pca_new_rot_ptr[PC_DIM_XY + 1][i] * weight_nw; } for (j = 1; j < PC_DIM_XY + 2; j++) { // first column - pca_new_rot_ptr2[j][0] = pca_new_rot_ptr[j][0] * weight_ne + pca_new_rot_ptr[j][PC_DIM_XY + 1] * weight_se + pca_new_rot_ptr[j - 1][0] * weight_nw; + pca_new_rot_ptr2[j][0] = pca_new_rot_ptr[j][0] * weight_ne + pca_new_rot_ptr[j][PC_DIM_XY + 1] * weight_se + + pca_new_rot_ptr[j - 1][0] * weight_nw; // all the rest for (i = 1; i < PC_DIM_XY + 2; i++) { - pca_new_rot_ptr2[j][i] = pca_new_rot_ptr[j][i] * weight_ne + pca_new_rot_ptr[j][i - 1] * weight_se + pca_new_rot_ptr[j - 1][i] * weight_nw; + pca_new_rot_ptr2[j][i] = pca_new_rot_ptr[j][i] * weight_ne + pca_new_rot_ptr[j][i - 1] * weight_se + + pca_new_rot_ptr[j - 1][i] * weight_nw; } } @@ -474,7 +474,7 @@ bool PosecellNetwork::path_integration(double vtrans, double vrot) // % unrotate the pose cell xy layer // pc.Posecells(:,:,dir_pc) = rot90(pca90, 4 - floor(dir * 2/pi)); rot90_square(posecells[dir_pc], PC_DIM_XY, 4 - (int)floor(dir * 2.0 / M_PI)); - //end + // end // end } @@ -538,7 +538,6 @@ bool PosecellNetwork::path_integration(double vtrans, double vrot) // assert(posecells[k][j][i] >= 0); } } - } // end } @@ -551,7 +550,7 @@ double PosecellNetwork::find_best() int i, j, k; double x = -1, y = -1, th = -1; - double * x_sums, *y_sums, *z_sums; + double* x_sums, *y_sums, *z_sums; double sum_x1, sum_x2, sum_y1, sum_y2; // % find the max activated cell @@ -660,12 +659,12 @@ double PosecellNetwork::find_best() return max; } -double * PosecellNetwork::get_cells(void) +double* PosecellNetwork::get_cells(void) { return posecells_memory; } -bool PosecellNetwork::set_cells(double * cells) +bool PosecellNetwork::set_cells(double* cells) { if (memcpy(posecells_memory, cells, posecells_memory_size) != NULL) return true; @@ -676,11 +675,12 @@ bool PosecellNetwork::set_cells(double * cells) double PosecellNetwork::get_delta_pc(double x, double y, double th) { double pc_th_corrected = best_th - vt_delta_pc_th; - if (pc_th_corrected < 0) - pc_th_corrected = PC_DIM_TH + pc_th_corrected; + if (pc_th_corrected < 0) + pc_th_corrected = PC_DIM_TH + pc_th_corrected; if (pc_th_corrected >= PC_DIM_TH) - pc_th_corrected = pc_th_corrected - PC_DIM_TH; - return sqrt(pow(get_min_delta(best_x, x, PC_DIM_XY), 2) + pow(get_min_delta(best_y, y, PC_DIM_XY), 2) + pow(get_min_delta(pc_th_corrected, th, PC_DIM_TH), 2)); + pc_th_corrected = pc_th_corrected - PC_DIM_TH; + return sqrt(pow(get_min_delta(best_x, x, PC_DIM_XY), 2) + pow(get_min_delta(best_y, y, PC_DIM_XY), 2) + + pow(get_min_delta(pc_th_corrected, th, PC_DIM_TH), 2)); } double PosecellNetwork::get_min_delta(double d1, double d2, double max) @@ -740,7 +740,7 @@ bool PosecellNetwork::pose_cell_inhibit_helper(int x, int y, int z) return true; } -void PosecellNetwork::circshift2d(double * array, double * array_buffer, int dimx, int dimy, int shiftx, int shifty) +void PosecellNetwork::circshift2d(double* array, double* array_buffer, int dimx, int dimy, int shiftx, int shifty) { if (shifty == 0) { @@ -786,7 +786,7 @@ void PosecellNetwork::circshift2d(double * array, double * array_buffer, int dim } } -int PosecellNetwork::rot90_square(double ** array, int dim, int rot) +int PosecellNetwork::rot90_square(double** array, int dim, int rot) { double centre = (double)(dim - 1) / 2.0f; @@ -824,7 +824,7 @@ int PosecellNetwork::rot90_square(double ** array, int dim, int rot) d = 0; break; default: - return 1; + return 1; } if (rot % 2 == 1) @@ -840,8 +840,8 @@ int PosecellNetwork::rot90_square(double ** array, int dim, int rot) { is1 = id; js1 = jd; - id = (int)(a * ((float)(is1) - centre) + b * ((float)(js1) - centre) + centre); - jd = (int)(c * ((float)(is1) - centre) + d * ((float)(js1) - centre) + centre); + id = (int)(a * ((float)(is1)-centre) + b * ((float)(js1)-centre) + centre); + jd = (int)(c * ((float)(is1)-centre) + d * ((float)(js1)-centre) + centre); tmp_new = array[jd][id]; array[jd][id] = tmp_old; tmp_old = tmp_new; @@ -857,7 +857,7 @@ int PosecellNetwork::rot90_square(double ** array, int dim, int rot) return true; } -int PosecellNetwork::generate_wrap(int * wrap, int start1, int end1, int start2, int end2, int start3, int end3) +int PosecellNetwork::generate_wrap(int* wrap, int start1, int end1, int start2, int end2, int start3, int end3) { int i, j; i = 0; @@ -880,16 +880,18 @@ int PosecellNetwork::generate_wrap(int * wrap, int start1, int end1, int start2, double PosecellNetwork::norm2d(double var, int x, int y, int z, int dim_centre) { - return 1.0 / (var * sqrt(2.0 * M_PI)) - * exp((-(x - dim_centre) * (x - dim_centre) - (y - dim_centre) * (y - dim_centre) - (z - dim_centre) * (z - dim_centre)) / (2.0 * var * var)); + return 1.0 / (var * sqrt(2.0 * M_PI)) * + exp((-(x - dim_centre) * (x - dim_centre) - (y - dim_centre) * (y - dim_centre) - + (z - dim_centre) * (z - dim_centre)) / + (2.0 * var * var)); } void PosecellNetwork::create_experience() { - PosecellVisualTemplate * pcvt = &visual_templates[current_vt]; + PosecellVisualTemplate* pcvt = &visual_templates[current_vt]; experiences.resize(experiences.size() + 1); current_exp = experiences.size() - 1; - PosecellExperience * exp = &experiences[current_exp]; + PosecellExperience* exp = &experiences[current_exp]; exp->x_pc = x(); exp->y_pc = y(); exp->th_pc = th(); @@ -897,10 +899,9 @@ void PosecellNetwork::create_experience() pcvt->exps.push_back(current_exp); } - PosecellNetwork::PosecellAction PosecellNetwork::get_action() { - PosecellExperience * experience; + PosecellExperience* experience; double delta_pc; PosecellAction action = NO_ACTION; @@ -908,9 +909,10 @@ PosecellNetwork::PosecellAction PosecellNetwork::get_action() { odo_update = false; vt_update = false; - - } else { - return action; + } + else + { + return action; } if (visual_templates.size() == 0) @@ -923,17 +925,19 @@ PosecellNetwork::PosecellAction PosecellNetwork::get_action() { create_experience(); action = CREATE_NODE; - } else { + } + else + { experience = &experiences[current_exp]; delta_pc = get_delta_pc(experience->x_pc, experience->y_pc, experience->th_pc); - PosecellVisualTemplate * pcvt = &visual_templates[current_vt]; + PosecellVisualTemplate* pcvt = &visual_templates[current_vt]; if (pcvt->exps.size() == 0) { create_experience(); action = CREATE_NODE; - } + } else if (delta_pc > EXP_DELTA_PC_THRESHOLD || current_vt != prev_vt) { // go through all the exps associated with the current view and find the one with the closest delta_pc @@ -943,7 +947,7 @@ PosecellNetwork::PosecellAction PosecellNetwork::get_action() double min_delta = DBL_MAX; double delta_pc; - // find the closest experience in pose cell space + // find the closest experience in pose cell space for (i = 0; i < pcvt->exps.size(); i++) { // make sure we aren't comparing to the current experience @@ -1010,24 +1014,24 @@ void PosecellNetwork::on_odo(double vtrans, double vrot, double time_diff_s) void PosecellNetwork::create_view_template() { - PosecellVisualTemplate * pcvt; + PosecellVisualTemplate* pcvt; visual_templates.resize(visual_templates.size() + 1); pcvt = &visual_templates[visual_templates.size() - 1]; pcvt->pc_x = x(); pcvt->pc_y = y(); pcvt->pc_th = th(); pcvt->decay = VT_ACTIVE_DECAY; - } void PosecellNetwork::on_view_template(unsigned int vt, double vt_rad) { - PosecellVisualTemplate * pcvt; + PosecellVisualTemplate* pcvt; + if (vt >= visual_templates.size()) { // must be a new template create_view_template(); - assert(vt == visual_templates.size()-1); + assert(vt == visual_templates.size() - 1); } else { @@ -1039,7 +1043,9 @@ void PosecellNetwork::on_view_template(unsigned int vt, double vt_rad) { if (vt != current_vt) { - } else { + } + else + { pcvt->decay += VT_ACTIVE_DECAY; } @@ -1047,18 +1053,18 @@ void PosecellNetwork::on_view_template(unsigned int vt, double vt_rad) double energy = PC_VT_INJECT_ENERGY * 1.0 / 30.0 * (30.0 - exp(1.2 * pcvt->decay)); if (energy > 0) { - vt_delta_pc_th = vt_rad / (2.0*M_PI) * PC_DIM_TH; - double pc_th_corrected = pcvt->pc_th + vt_rad / (2.0*M_PI) * PC_DIM_TH; - if (pc_th_corrected < 0) - pc_th_corrected = PC_DIM_TH + pc_th_corrected; - if (pc_th_corrected >= PC_DIM_TH) - pc_th_corrected = pc_th_corrected - PC_DIM_TH; + vt_delta_pc_th = vt_rad / (2.0 * M_PI) * PC_DIM_TH; + double pc_th_corrected = pcvt->pc_th + vt_rad / (2.0 * M_PI) * PC_DIM_TH; + if (pc_th_corrected < 0) + pc_th_corrected = PC_DIM_TH + pc_th_corrected; + if (pc_th_corrected >= PC_DIM_TH) + pc_th_corrected = pc_th_corrected - PC_DIM_TH; inject((int)pcvt->pc_x, (int)pcvt->pc_y, (int)pc_th_corrected, energy); } } } - for (unsigned int i=0; i < visual_templates.size(); i++) + for (unsigned int i = 0; i < visual_templates.size(); i++) { visual_templates[i].decay -= PC_VT_RESTORE; if (visual_templates[i].decay < VT_ACTIVE_DECAY) @@ -1068,8 +1074,7 @@ void PosecellNetwork::on_view_template(unsigned int vt, double vt_rad) prev_vt = current_vt; current_vt = vt; -vt_update = true; + vt_update = true; } -} // namespace ratslam - +} // namespace ratslam diff --git a/src/ratslam/posecell_network.h b/src/ratslam/posecell_network.h index ce3bbeb..4004c2b 100644 --- a/src/ratslam/posecell_network.h +++ b/src/ratslam/posecell_network.h @@ -37,11 +37,13 @@ #define _POSE_CELL_NETWORK_HPP #define _USE_MATH_DEFINES + #include "math.h" #include #include + using boost::property_tree::ptree; typedef double Posecell; @@ -53,41 +55,50 @@ typedef double Posecell; namespace ratslam { - struct PosecellVisualTemplate { - unsigned int id; - double pc_x, pc_y, pc_th; - double decay; - std::vector exps; - - template - void serialize(Archive& ar, const unsigned int version) - { - ar & id; - ar & pc_x & pc_y & pc_th; - ar & decay; - } + unsigned int id; + double pc_x, pc_y, pc_th; + double decay; + std::vector exps; + template + void serialize(Archive& ar, const unsigned int version) + { + ar& id; + ar& pc_x& pc_y& pc_th; + ar& decay; + } }; -struct PosecellExperience { - +struct PosecellExperience +{ double x_pc, y_pc, th_pc; int vt_id; -}; + template + void serialize(Archive& ar, const unsigned int version) + { + ar& x_pc& y_pc& th_pc; + ar& vt_id; + } +}; class PosecellNetwork { - public: - friend class PosecellScene; - enum PosecellAction {NO_ACTION = 0, CREATE_NODE, CREATE_EDGE, SET_NODE}; + enum PosecellAction + { + NO_ACTION = 0, + CREATE_NODE, + CREATE_EDGE, + SET_NODE + }; PosecellNetwork(ptree settings); + ~PosecellNetwork(); void on_odo(double vtrans, double vrot, double time_diff_s); @@ -101,86 +112,101 @@ class PosecellNetwork { return best_x; } + double y() { return best_y; } + double th() { return best_th; } // get and set all the cells as one array - double * get_cells(); - bool set_cells(double * cells); + double* get_cells(); + bool set_cells(double* cells); // access to some of the constants specified in // RatSLAM properties. double get_delta_pc(double x, double y, double th); - unsigned int get_current_exp_id() { return current_exp; } - - double get_relative_rad() { return vt_delta_pc_th * 2.0 * M_PI / PC_DIM_TH; } - - template - void save(Archive& ar, const unsigned int version) const - { - ar & PC_DIM_XY; - ar & PC_DIM_TH; - ar & PC_W_E_DIM; - ar & PC_W_I_DIM; - ar & PC_W_E_VAR; - ar & PC_W_I_VAR; - ar & PC_GLOBAL_INHIB; - - ar & VT_ACTIVE_DECAY; - ar & PC_VT_RESTORE; - - ar & best_x; - ar & best_y; - ar & best_th; - - int i, j, k; - for (k = 0; k < PC_DIM_TH; k++) - for (j = 0; j < PC_DIM_XY; j++) - for (i = 0; i < PC_DIM_XY; i++) - ar & posecells[k][j][i]; - - } - - template - void load(Archive& ar, const unsigned int version) - { - ar & PC_DIM_XY; - ar & PC_DIM_TH; - ar & PC_W_E_DIM; - ar & PC_W_I_DIM; - ar & PC_W_E_VAR; - ar & PC_W_I_VAR; - ar & PC_GLOBAL_INHIB; - - ar & VT_ACTIVE_DECAY; - ar & PC_VT_RESTORE; - - ar & best_x; - ar & best_y; - ar & best_th; - - pose_cell_builder(); - - int i, j, k; - for (k = 0; k < PC_DIM_TH; k++) - for (j = 0; j < PC_DIM_XY; j++) - for (i = 0; i < PC_DIM_XY; i++) - ar & posecells[k][j][i]; - } + unsigned int get_current_exp_id() + { + return current_exp; + } + + double get_relative_rad() + { + return vt_delta_pc_th * 2.0 * M_PI / PC_DIM_TH; + } + + template + void save(Archive& ar, const unsigned int version) const + { + ar& PC_DIM_XY; + ar& PC_DIM_TH; + ar& PC_W_E_DIM; + ar& PC_W_I_DIM; + ar& PC_W_E_VAR; + ar& PC_W_I_VAR; + ar& PC_GLOBAL_INHIB; + + ar& VT_ACTIVE_DECAY; + ar& PC_VT_RESTORE; + + ar& best_x; + ar& best_y; + ar& best_th; + + ar& visual_templates; + ar& experiences; + + int i, j, k; + for (k = 0; k < PC_DIM_TH; k++) + for (j = 0; j < PC_DIM_XY; j++) + for (i = 0; i < PC_DIM_XY; i++) + ar& posecells[k][j][i]; + } + + template + void load(Archive& ar, const unsigned int version) + { + ar& PC_DIM_XY; + ar& PC_DIM_TH; + ar& PC_W_E_DIM; + ar& PC_W_I_DIM; + ar& PC_W_E_VAR; + ar& PC_W_I_VAR; + ar& PC_GLOBAL_INHIB; + + ar& VT_ACTIVE_DECAY; + ar& PC_VT_RESTORE; + + ar& best_x; + ar& best_y; + ar& best_th; + + ar& visual_templates; + ar& experiences; + + pose_cell_builder(); + + int i, j, k; + for (k = 0; k < PC_DIM_TH; k++) + for (j = 0; j < PC_DIM_XY; j++) + for (i = 0; i < PC_DIM_XY; i++) + ar& posecells[k][j][i]; + } BOOST_SERIALIZATION_SPLIT_MEMBER() + private: friend class boost::serialization::access; void create_experience(); + void create_view_template(); // inject energy into a specific point in the network @@ -189,6 +215,7 @@ class PosecellNetwork // locally excite and inhibit points. Excite spreads energy and // inhibit compresses. bool excite(); + bool inhibit(); // global inhibition @@ -209,15 +236,25 @@ class PosecellNetwork { ; } - PosecellNetwork(const PosecellNetwork & other); - const PosecellNetwork & operator=(const PosecellNetwork & other); + + PosecellNetwork(const PosecellNetwork& other); + + const PosecellNetwork& operator=(const PosecellNetwork& other); + void pose_cell_builder(); + bool pose_cell_excite_helper(int x, int y, int z); + bool pose_cell_inhibit_helper(int x, int y, int z); - void circshift2d(double * array, double * array_buffer, int dimx, int dimy, int shiftx, int shifty); - int rot90_square(double ** array, int dim, int rot); - int generate_wrap(int * wrap, int start1, int end1, int start2, int end2, int start3, int end3); + + void circshift2d(double* array, double* array_buffer, int dimx, int dimy, int shiftx, int shifty); + + int rot90_square(double** array, int dim, int rot); + + int generate_wrap(int* wrap, int start1, int end1, int start2, int end2, int start3, int end3); + double norm2d(double var, int x, int y, int z, int dim_centre); + double get_min_delta(double d1, double d2, double max); int PC_DIM_XY; @@ -244,34 +281,34 @@ class PosecellNetwork bool odo_update; bool vt_update; - Posecell *** posecells; - Posecell * posecells_memory; + Posecell*** posecells; + Posecell* posecells_memory; int posecells_memory_size; int posecells_elements; - Posecell *** pca_new; - Posecell * pca_new_memory; - Posecell ** pca_new_rot_ptr; - Posecell ** pca_new_rot_ptr2; - Posecell * posecells_plane_th; - double * PC_W_EXCITE; - double * PC_W_INHIB; + Posecell*** pca_new; + Posecell* pca_new_memory; + Posecell** pca_new_rot_ptr; + Posecell** pca_new_rot_ptr2; + Posecell* posecells_plane_th; + double* PC_W_EXCITE; + double* PC_W_INHIB; int PC_W_E_DIM_HALF; int PC_W_I_DIM_HALF; - int * PC_E_XY_WRAP; - int * PC_E_TH_WRAP; - int * PC_I_XY_WRAP; - int * PC_I_TH_WRAP; + int* PC_E_XY_WRAP; + int* PC_E_TH_WRAP; + int* PC_I_XY_WRAP; + int* PC_I_TH_WRAP; int PC_CELLS_TO_AVG; - int * PC_AVG_XY_WRAP; - int * PC_AVG_TH_WRAP; + int* PC_AVG_XY_WRAP; + int* PC_AVG_TH_WRAP; - double * PC_XY_SUM_SIN_LOOKUP; - double * PC_XY_SUM_COS_LOOKUP; - double * PC_TH_SUM_SIN_LOOKUP; - double * PC_TH_SUM_COS_LOOKUP; + double* PC_XY_SUM_SIN_LOOKUP; + double* PC_XY_SUM_COS_LOOKUP; + double* PC_TH_SUM_SIN_LOOKUP; + double* PC_TH_SUM_COS_LOOKUP; double PC_C_SIZE_TH; @@ -280,9 +317,7 @@ class PosecellNetwork unsigned int current_vt, prev_vt; unsigned int current_exp, prev_exp; - }; - } -#endif // _POSE_CELL_NETWORK_HPP +#endif // _POSE_CELL_NETWORK_HPP diff --git a/src/ratslam/visual_odometry.cpp b/src/ratslam/visual_odometry.cpp index c0fbe3f..05910cf 100644 --- a/src/ratslam/visual_odometry.cpp +++ b/src/ratslam/visual_odometry.cpp @@ -34,7 +34,6 @@ namespace ratslam { - VisualOdometry::VisualOdometry(ptree settings) { get_setting_from_ptree(VTRANS_IMAGE_X_MIN, settings, "vtrans_image_x_min", 0); @@ -61,7 +60,8 @@ VisualOdometry::VisualOdometry(ptree settings) first = true; } -void VisualOdometry::on_image(const unsigned char * data, bool greyscale, unsigned int image_width, unsigned int image_height, double *vtrans_ms, double *vrot_rads) +void VisualOdometry::on_image(const unsigned char* data, bool greyscale, unsigned int image_width, + unsigned int image_height, double* vtrans_ms, double* vrot_rads) { double dummy; @@ -81,14 +81,17 @@ void VisualOdometry::on_image(const unsigned char * data, bool greyscale, unsign first = false; } - convert_view_to_view_template(&vtrans_profile[0], data, greyscale, VTRANS_IMAGE_X_MIN, VTRANS_IMAGE_X_MAX, VTRANS_IMAGE_Y_MIN, VTRANS_IMAGE_Y_MAX); + convert_view_to_view_template(&vtrans_profile[0], data, greyscale, VTRANS_IMAGE_X_MIN, VTRANS_IMAGE_X_MAX, + VTRANS_IMAGE_Y_MIN, VTRANS_IMAGE_Y_MAX); visual_odo(&vtrans_profile[0], vtrans_profile.size(), &vtrans_prev_profile[0], vtrans_ms, &dummy); - convert_view_to_view_template(&vrot_profile[0], data, greyscale, VROT_IMAGE_X_MIN, VROT_IMAGE_X_MAX, VROT_IMAGE_Y_MIN, VROT_IMAGE_Y_MAX); + convert_view_to_view_template(&vrot_profile[0], data, greyscale, VROT_IMAGE_X_MIN, VROT_IMAGE_X_MAX, VROT_IMAGE_Y_MIN, + VROT_IMAGE_Y_MAX); visual_odo(&vrot_profile[0], vrot_profile.size(), &vrot_prev_profile[0], &dummy, vrot_rads); } -void VisualOdometry::visual_odo(double *data, unsigned short width, double *olddata, double *vtrans_ms, double *vrot_rads) +void VisualOdometry::visual_odo(double* data, unsigned short width, double* olddata, double* vtrans_ms, + double* vrot_rads) { double mindiff = 1e6; double minoffset = 0; @@ -143,15 +146,14 @@ void VisualOdometry::visual_odo(double *data, unsigned short width, double *oldd { olddata[i] = data[i]; } - *vrot_rads = minoffset * CAMERA_FOV_DEG / IMAGE_WIDTH * CAMERA_HZ * M_PI / 180.0; - *vtrans_ms = mindiff * VTRANS_SCALING; + *vrot_rads = minoffset* CAMERA_FOV_DEG / IMAGE_WIDTH* CAMERA_HZ* M_PI / 180.0; + *vtrans_ms = mindiff* VTRANS_SCALING; if (*vtrans_ms > VTRANS_MAX) *vtrans_ms = VTRANS_MAX; - } -void VisualOdometry::convert_view_to_view_template(double *current_view, const unsigned char *view_rgb, bool grayscale, int X_RANGE_MIN, int X_RANGE_MAX, int Y_RANGE_MIN, - int Y_RANGE_MAX) +void VisualOdometry::convert_view_to_view_template(double* current_view, const unsigned char* view_rgb, bool grayscale, + int X_RANGE_MIN, int X_RANGE_MAX, int Y_RANGE_MIN, int Y_RANGE_MAX) { unsigned int TEMPLATE_Y_SIZE = 1; unsigned int TEMPLATE_X_SIZE = X_RANGE_MAX - X_RANGE_MIN; @@ -170,9 +172,11 @@ void VisualOdometry::convert_view_to_view_template(double *current_view, const u if (grayscale) { - for (int y_block = Y_RANGE_MIN, y_block_count = 0; y_block_count < TEMPLATE_Y_SIZE; y_block += y_block_size, y_block_count++) + for (int y_block = Y_RANGE_MIN, y_block_count = 0; y_block_count < TEMPLATE_Y_SIZE; + y_block += y_block_size, y_block_count++) { - for (int x_block = X_RANGE_MIN, x_block_count = 0; x_block_count < TEMPLATE_X_SIZE; x_block += x_block_size, x_block_count++) + for (int x_block = X_RANGE_MIN, x_block_count = 0; x_block_count < TEMPLATE_X_SIZE; + x_block += x_block_size, x_block_count++) { for (int x = x_block; x < (x_block + x_block_size); x++) { @@ -190,16 +194,19 @@ void VisualOdometry::convert_view_to_view_template(double *current_view, const u } else { - for (int y_block = Y_RANGE_MIN, y_block_count = 0; y_block_count < TEMPLATE_Y_SIZE; y_block += y_block_size, y_block_count++) + for (int y_block = Y_RANGE_MIN, y_block_count = 0; y_block_count < TEMPLATE_Y_SIZE; + y_block += y_block_size, y_block_count++) { - for (int x_block = X_RANGE_MIN, x_block_count = 0; x_block_count < TEMPLATE_X_SIZE; x_block += x_block_size, x_block_count++) + for (int x_block = X_RANGE_MIN, x_block_count = 0; x_block_count < TEMPLATE_X_SIZE; + x_block += x_block_size, x_block_count++) { for (int x = x_block; x < (x_block + x_block_size); x++) { for (int y = y_block; y < (y_block + y_block_size); y++) { pos = (x + y * IMAGE_WIDTH) * 3; - current_view[data_next] += ((double)(view_rgb[pos]) + (double)(view_rgb[pos + 1]) + (double)(view_rgb[pos + 2])); + current_view[data_next] += + ((double)(view_rgb[pos]) + (double)(view_rgb[pos + 1]) + (double)(view_rgb[pos + 2])); } } current_view[data_next] /= (255.0 * 3.0); @@ -208,9 +215,6 @@ void VisualOdometry::convert_view_to_view_template(double *current_view, const u } } } - -} - } -; +}; // namespace ratslam diff --git a/src/ratslam/visual_odometry.h b/src/ratslam/visual_odometry.h index bb084be..7b1c6cc 100644 --- a/src/ratslam/visual_odometry.h +++ b/src/ratslam/visual_odometry.h @@ -27,6 +27,7 @@ */ #include + using boost::property_tree::ptree; #include @@ -37,50 +38,52 @@ using boost::property_tree::ptree; #include #include -namespace ratslam { - +namespace ratslam +{ class VisualOdometry { public: - VisualOdometry(ptree settings); - void on_image(const unsigned char * data, bool greyscale, unsigned int image_width, unsigned int image_height, double *vtrans_ms, double *vrot_rads); + void on_image(const unsigned char* data, bool greyscale, unsigned int image_width, unsigned int image_height, + double* vtrans_ms, double* vrot_rads); - template - void serialize(Archive& ar, const unsigned int version) - { - ar & IMAGE_HEIGHT; - ar & IMAGE_WIDTH; + template + void serialize(Archive& ar, const unsigned int version) + { + ar& IMAGE_HEIGHT; + ar& IMAGE_WIDTH; - ar & VTRANS_IMAGE_X_MIN; - ar & VTRANS_IMAGE_X_MAX; - ar & VTRANS_IMAGE_Y_MIN; - ar & VTRANS_IMAGE_Y_MAX; + ar& VTRANS_IMAGE_X_MIN; + ar& VTRANS_IMAGE_X_MAX; + ar& VTRANS_IMAGE_Y_MIN; + ar& VTRANS_IMAGE_Y_MAX; - ar & VROT_IMAGE_X_MIN; - ar & VROT_IMAGE_X_MAX; - ar & VROT_IMAGE_Y_MIN; - ar & VROT_IMAGE_Y_MAX; + ar& VROT_IMAGE_X_MIN; + ar& VROT_IMAGE_X_MAX; + ar& VROT_IMAGE_Y_MIN; + ar& VROT_IMAGE_Y_MAX; - ar & CAMERA_FOV_DEG; - ar & CAMERA_HZ; + ar& CAMERA_FOV_DEG; + ar& CAMERA_HZ; - ar & VTRANS_SCALING; - ar & VTRANS_MAX; + ar& VTRANS_SCALING; + ar& VTRANS_MAX; - ar & vtrans_profile; - ar & vrot_profile; + ar& vtrans_profile; + ar& vrot_profile; - ar & vtrans_prev_profile; - ar & vrot_prev_profile; + ar& vtrans_prev_profile; + ar& vrot_prev_profile; - ar & first; - } + ar& first; + } private: - - VisualOdometry() {;} + VisualOdometry() + { + ; + } int IMAGE_HEIGHT; int IMAGE_WIDTH; @@ -109,10 +112,10 @@ class VisualOdometry bool first; - void visual_odo(double *data, unsigned short width, double *olddata, double *vtrans_ms, double *vrot_rads); - - void convert_view_to_view_template(double *current_view, const unsigned char *view_rgb, bool grayscale, int X_RANGE_MIN, int X_RANGE_MAX, int Y_RANGE_MIN, int Y_RANGE_MAX); + void visual_odo(double* data, unsigned short width, double* olddata, double* vtrans_ms, double* vrot_rads); + void convert_view_to_view_template(double* current_view, const unsigned char* view_rgb, bool grayscale, + int X_RANGE_MIN, int X_RANGE_MAX, int Y_RANGE_MIN, int Y_RANGE_MAX); }; -}; // namespace ratslam +}; // namespace ratslam diff --git a/tests/irat_aus.test b/tests/irat_aus.test new file mode 100644 index 0000000..d682e57 --- /dev/null +++ b/tests/irat_aus.test @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +