From d69c172b5558f7f48d6e2f35e31dcdb39c5cc98b Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Fri, 12 May 2017 20:46:37 +0200 Subject: [PATCH 01/16] Added load/save map and tf broadcast feature. This closes #1 and closes #2. * [mod] added boost archive feature to PosecellVisualTemplate class * [mod] added tf map->odom publisher to experience mapper/localizer * [mod] added save/load functions to experience map * [mod] added save/load functions to local view matcher * [mod] added save/load functions to pose cell network --- src/main_em.cpp | 187 +++++++++++++++++++++------ src/main_lv.cpp | 69 ++++++++-- src/main_pc.cpp | 89 ++++++++++--- src/main_vo.cpp | 4 +- src/ratslam/posecell_network.h | 225 ++++++++++++++++++--------------- 5 files changed, 405 insertions(+), 169 deletions(-) diff --git a/src/main_em.cpp b/src/main_em.cpp index f221261..2711926 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,50 @@ #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) -{ - 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); +std::string map_frame = std::string("map"); +std::string odom_frame = std::string("odom"); +std::string base_frame = std::string("base_footprint"); +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); static ros::Time prev_time(0); @@ -76,7 +105,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 +118,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 +137,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 +152,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 +173,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 +192,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 +236,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; @@ -236,53 +272,131 @@ 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)""); + get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string) ""); - if (!ros::isInitialized()) - { - ros::init(argc, argv, "RatSLAMExperienceMap"); - } 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); + + // 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()); + } - ratslam::ExperienceMap * em = new ratslam::ExperienceMap(ratslam_settings); - + // 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(), - 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()); + // subs + ros::Subscriber sub_odometry = node.subscribe( + "odom", 0, odo_callback, 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 +412,3 @@ int main(int argc, char * argv[]) return 0; } - diff --git a/src/main_lv.cpp b/src/main_lv.cpp index a9b63d9..5e11fad 100644 --- a/src/main_lv.cpp +++ b/src/main_lv.cpp @@ -27,6 +27,10 @@ */ #include + +#include +#include + using namespace std; #include @@ -52,24 +56,27 @@ ratslam::LocalViewScene *lvs = NULL; bool use_graphics; #endif +ros::Publisher pub_vt; using namespace ratslam; 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,6 +86,35 @@ void image_callback(sensor_msgs::ImageConstPtr image, ros::Publisher * pub_vt) #endif } +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"); @@ -98,19 +134,34 @@ 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)""); get_setting_child(ratslam_settings, settings, "ratslam", true); + lv = new ratslam::LocalViewMatch(ratslam_settings); - if (!ros::isInitialized()) - { - ros::init(argc, argv, "RatSLAMViewTemplate"); - } + // 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); + + // try to load lvm + if (!load_lvm(lvm_file_path)) { + ROS_WARN("Could not load LocalViewMatch from file \"%s\"", lvm_file_path.c_str()); + } - 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", 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; diff --git a/src/main_pc.cpp b/src/main_pc.cpp index 0411d19..3357a2d 100644 --- a/src/main_pc.cpp +++ b/src/main_pc.cpp @@ -26,19 +26,18 @@ * 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" @@ -46,11 +45,17 @@ 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); @@ -69,7 +74,7 @@ void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::PosecellNetwork *pc, 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); + 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); } @@ -85,7 +90,7 @@ void odo_callback(nav_msgs::OdometryConstPtr odo, ratslam::PosecellNetwork *pc, 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); @@ -98,7 +103,35 @@ void template_callback(ratslam_ros::ViewTemplateConstPtr vt, ratslam::PosecellNe 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[]) @@ -120,28 +153,44 @@ 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()) - { - ros::init(argc, argv, "RatSLAMPoseCells"); - } + // 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); + + // 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()); + } + // pubs + pub_pc = node.advertise(topic_root + "/PoseCell/TopologicalAction", 0); + // subs + ros::Subscriber sub_odometry = node.subscribe( + "odom", 0, odo_callback, ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub_template = node.subscribe( + topic_root + "/LocalView/Template", 0, template_callback, ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); - ratslam::PosecellNetwork * pc = new ratslam::PosecellNetwork(ratslam_settings); - ros::Publisher pub_pc = node.advertise(topic_root + "/PoseCell/TopologicalAction", 0); + // timers + ros::Timer pcn_save_timer = node.createTimer(ros::Duration(pcn_save_period), save_pcn_timer_callback); - ros::Subscriber sub_odometry = node.subscribe(topic_root + "/odom", 0, boost::bind(odo_callback, _1, pc, &pub_pc), 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()); #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..59053df 100644 --- a/src/main_vo.cpp +++ b/src/main_vo.cpp @@ -93,10 +93,10 @@ int main(int argc, char * argv[]) } ros::NodeHandle node; - pub_vo = node.advertise(topic_root + "/odom", 0); + pub_vo = node.advertise("odom", 0); 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", 1, image_callback); ros::spin(); diff --git a/src/ratslam/posecell_network.h b/src/ratslam/posecell_network.h index ce3bbeb..61a7909 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; @@ -51,23 +53,20 @@ typedef double Posecell; #include #include -namespace ratslam -{ +namespace ratslam { -struct PosecellVisualTemplate -{ - unsigned int id; - double pc_x, pc_y, pc_th; - double decay; - std::vector exps; +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; - } + template + void serialize(Archive &ar, const unsigned int version) { + ar & id; + ar & pc_x & pc_y & pc_th; + ar & decay; + } }; @@ -75,19 +74,27 @@ 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 -{ +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); @@ -97,22 +104,22 @@ class PosecellNetwork PosecellAction get_action(); // these updated by find_best() - double x() - { + double x() { return best_x; } - double y() - { + + double y() { return best_y; } - double th() - { + + 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 @@ -124,63 +131,69 @@ class PosecellNetwork 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]; - - } + 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; - - 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]; - } + 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 +202,7 @@ class PosecellNetwork // locally excite and inhibit points. Excite spreads energy and // inhibit compresses. bool excite(); + bool inhibit(); // global inhibition @@ -205,19 +219,28 @@ class PosecellNetwork // packet. double find_best(); - PosecellNetwork() - { + 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 +267,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; From 185e7db5a308b68fb5ee8ea6df642023a5fda2a2 Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Fri, 12 May 2017 21:00:35 +0200 Subject: [PATCH 02/16] Reformatted code according ROS guidelines. This closes #3. * [mod] reformatted code style using clang-format --- src/graphics/experience_map_scene.h | 118 +++++------ src/graphics/local_view_scene.h | 82 ++++---- src/graphics/path_node.h | 264 ++++++++++++----------- src/graphics/posecell_scene.h | 158 ++++++++------ src/main_em.cpp | 32 +-- src/main_lv.cpp | 30 +-- src/main_pc.cpp | 60 +++--- src/main_vo.cpp | 10 +- src/ratslam/experience_map.cpp | 60 +++--- src/ratslam/experience_map.h | 119 ++++++----- src/ratslam/local_view_match.cpp | 311 ++++++++++++++-------------- src/ratslam/local_view_match.h | 83 ++++---- src/ratslam/posecell_network.cpp | 165 ++++++++------- src/ratslam/posecell_network.h | 148 +++++++------ src/ratslam/visual_odometry.cpp | 42 ++-- src/ratslam/visual_odometry.h | 69 +++--- 16 files changed, 896 insertions(+), 855 deletions(-) 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 2711926..d8e3c6e 100644 --- a/src/main_em.cpp +++ b/src/main_em.cpp @@ -68,7 +68,9 @@ visualization_msgs::Marker em_marker; ratslam::ExperienceMap* em; #ifdef HAVE_IRRLICHT + #include "graphics/experience_map_scene.h" + ratslam::ExperienceMapScene* ems; bool use_graphics; #endif @@ -88,7 +90,8 @@ std::string map_file_path = std::string("ratslam-latest.bmap"); * \param odo The incoming odometry message. * \param em Pointer to current experience map. */ -void odo_callback(nav_msgs::OdometryConstPtr odo) { +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); @@ -251,7 +254,6 @@ void action_callback(ratslam_ros::TopologicalActionConstPtr action) 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; @@ -316,7 +318,8 @@ void save_map_timer_callback(const ros::TimerEvent& event) bool load_map(const std::string& file_path) { - try { + try + { // create and open an archive for input std::ifstream ifs(file_path.c_str()); boost::archive::binary_iarchive binary_archive(ifs); @@ -324,7 +327,9 @@ bool load_map(const std::string& file_path) binary_archive >> *em; // archive and stream closed when destructors are called return true; - } catch (...) { + } + catch (...) + { return false; } } @@ -374,7 +379,8 @@ int main(int argc, char* argv[]) em = new ratslam::ExperienceMap(ratslam_settings); // try to load map - if (!load_map(map_file_path)) { + if (!load_map(map_file_path)) + { ROS_WARN("Could not load map from file \"%s\"", map_file_path.c_str()); } @@ -385,14 +391,14 @@ int main(int argc, char* argv[]) pub_goal_path = node.advertise(topic_root + "/ExperienceMap/PathToGoal", 1); // subs - ros::Subscriber sub_odometry = node.subscribe( - "odom", 0, odo_callback, 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_odometry = node.subscribe("odom", 0, odo_callback, 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()); // timers ros::Timer tf_update_timer = priv_node.createTimer(ros::Duration(1.0 / tf_update_rate), tf_update_timer_callback); diff --git a/src/main_lv.cpp b/src/main_lv.cpp index 5e11fad..a5ec52e 100644 --- a/src/main_lv.cpp +++ b/src/main_lv.cpp @@ -51,15 +51,17 @@ 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; @@ -102,7 +104,8 @@ void save_lvm_timer_callback(const ros::TimerEvent& event) bool load_lvm(const std::string& file_path) { - try { + try + { // create and open an archive for input std::ifstream ifs(file_path.c_str()); boost::archive::binary_iarchive binary_archive(ifs); @@ -110,12 +113,14 @@ bool load_lvm(const std::string& file_path) binary_archive >> *lv; // archive and stream closed when destructors are called return true; - } catch (...) { + } + 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"); @@ -132,7 +137,7 @@ 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)""); + get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string) ""); get_setting_child(ratslam_settings, settings, "ratslam", true); lv = new ratslam::LocalViewMatch(ratslam_settings); @@ -149,7 +154,8 @@ int main(int argc, char * argv[]) priv_node.param("lvm_file_path", lvm_file_path, lvm_file_path); // try to load lvm - if (!load_lvm(lvm_file_path)) { + if (!load_lvm(lvm_file_path)) + { ROS_WARN("Could not load LocalViewMatch from file \"%s\"", lvm_file_path.c_str()); } @@ -164,11 +170,11 @@ int main(int argc, char * argv[]) 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 3357a2d..a381c83 100644 --- a/src/main_pc.cpp +++ b/src/main_pc.cpp @@ -40,8 +40,10 @@ #include "ratslam/posecell_network.h" #if HAVE_IRRLICHT + #include "graphics/posecell_scene.h" -ratslam::PosecellScene *pcs; + +ratslam::PosecellScene* pcs; bool use_graphics; #endif @@ -57,7 +59,8 @@ double pcn_save_period = 10.0; 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); @@ -73,18 +76,19 @@ void odo_callback(nav_msgs::OdometryConstPtr odo) 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(); + 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); + 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; @@ -92,16 +96,17 @@ void odo_callback(nav_msgs::OdometryConstPtr odo) 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 } @@ -121,7 +126,8 @@ void save_pcn_timer_callback(const ros::TimerEvent& event) bool load_pcn(const std::string& file_path) { - try { + try + { // create and open an archive for input std::ifstream ifs(file_path.c_str()); boost::archive::binary_iarchive binary_archive(ifs); @@ -129,12 +135,14 @@ bool load_pcn(const std::string& file_path) pc->load(binary_archive, 1); // archive and stream closed when destructors are called return true; - } catch (...) { + } + 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"); @@ -168,7 +176,8 @@ int main(int argc, char * argv[]) pc = new ratslam::PosecellNetwork(ratslam_settings); // try to load pcn - if (!load_pcn(pcn_file_path)) { + if (!load_pcn(pcn_file_path)) + { ROS_WARN("Could not load PoseCellNetwork from file \"%s\"", pcn_file_path.c_str()); } @@ -176,10 +185,11 @@ int main(int argc, char * argv[]) pub_pc = node.advertise(topic_root + "/PoseCell/TopologicalAction", 0); // subs - ros::Subscriber sub_odometry = node.subscribe( - "odom", 0, odo_callback, ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); - ros::Subscriber sub_template = node.subscribe( - topic_root + "/LocalView/Template", 0, template_callback, ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay()); + ros::Subscriber sub_odometry = node.subscribe("odom", 0, odo_callback, 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); @@ -190,7 +200,7 @@ int main(int argc, char * argv[]) 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 59053df..8023a03 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"); 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 61a7909..4004c2b 100644 --- a/src/ratslam/posecell_network.h +++ b/src/ratslam/posecell_network.h @@ -53,44 +53,48 @@ typedef double Posecell; #include #include -namespace ratslam { - -struct PosecellVisualTemplate { +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; + 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; + template + void serialize(Archive& ar, const unsigned int version) + { + ar& x_pc& y_pc& th_pc; + ar& vt_id; } }; - -class PosecellNetwork { - +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); @@ -104,15 +108,18 @@ class PosecellNetwork { PosecellAction get_action(); // these updated by find_best() - double x() { + double x() + { return best_x; } - double y() { + double y() + { return best_y; } - double th() { + double th() + { return best_th; } @@ -121,62 +128,68 @@ class PosecellNetwork { 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; } + 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; } + 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; + 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& VT_ACTIVE_DECAY; + ar& PC_VT_RESTORE; - ar & best_x; - ar & best_y; - ar & best_th; + ar& best_x; + ar& best_y; + ar& best_th; - ar & visual_templates; - ar & experiences; + 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]; - + 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; + 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& VT_ACTIVE_DECAY; + ar& PC_VT_RESTORE; - ar & best_x; - ar & best_y; - ar & best_th; + ar& best_x; + ar& best_y; + ar& best_th; - ar & visual_templates; - ar & experiences; + ar& visual_templates; + ar& experiences; pose_cell_builder(); @@ -184,7 +197,7 @@ class PosecellNetwork { 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]; + ar& posecells[k][j][i]; } BOOST_SERIALIZATION_SPLIT_MEMBER() @@ -219,13 +232,14 @@ class PosecellNetwork { // packet. double find_best(); - PosecellNetwork() { + PosecellNetwork() + { ; } - PosecellNetwork(const PosecellNetwork &other); + PosecellNetwork(const PosecellNetwork& other); - const PosecellNetwork &operator=(const PosecellNetwork &other); + const PosecellNetwork& operator=(const PosecellNetwork& other); void pose_cell_builder(); @@ -303,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 From ef68617491e1d95a19bb13a0ba546fd762eab5dd Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Fri, 12 May 2017 21:02:14 +0200 Subject: [PATCH 03/16] Added linking against boost libraries. * [mod] modified CMakeLists.txt to link against Boost --- CMakeLists.txt | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e63f966..b8c614a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(Boost REQUIRED COMPONENTS serialization) find_package(Irrlicht REQUIRED) find_package(OpenGL REQUIRED) - ## Generate messages in the 'msg' folder add_message_files( FILES @@ -69,26 +68,29 @@ add_library(ratslam src/ratslam/experience_map.cpp src/ratslam/posecell_network. add_definitions("-DHAVE_IRRLICHT") add_executable(ratslam_lv src/main_lv.cpp) -target_link_libraries(ratslam_lv +target_link_libraries(ratslam_lv ${catkin_LIBRARIES} ratslam ${IRRLICHT_LIBRARIES} ${OPENGL_LIBRARIES} - ${OpenCV_LIBRARIES}) + ${OpenCV_LIBRARIES} + ${Boost_LIBRARIES}) add_executable(ratslam_pc src/main_pc.cpp) target_link_libraries(ratslam_pc - ${catkin_LIBRARIES} + ${catkin_LIBRARIES} ratslam ${IRRLICHT_LIBRARIES} - ${OPENGL_LIBRARIES}) + ${OPENGL_LIBRARIES} + ${Boost_LIBRARIES}) add_executable(ratslam_em src/main_em.cpp) target_link_libraries(ratslam_em ${catkin_LIBRARIES} ratslam ${IRRLICHT_LIBRARIES} - ${OPENGL_LIBRARIES}) + ${OPENGL_LIBRARIES} + ${Boost_LIBRARIES}) add_executable(ratslam_vo src/main_vo.cpp) target_link_libraries(ratslam_vo From a7909d96856b80c807fcf42928efd985261bbbef Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Sat, 13 May 2017 15:59:30 +0200 Subject: [PATCH 04/16] Added naive path follower. This closes #5. * [add] added python path follow controller node --- script/path_follower.py | 151 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 151 insertions(+) create mode 100755 script/path_follower.py 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() From c752a66990b8d042b47dee7281d0169336c8363f Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Sun, 14 May 2017 00:03:59 +0200 Subject: [PATCH 05/16] Added Travis CI configuration. * [add] added travis CI configuration provided by ROS Industrial team --- .travis.yml | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..f365953 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,26 @@ +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="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 +matrix: + allow_failures: + - 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 +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 From a7e5ea826ba81161df0cc4782da7518c391badb3 Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Sun, 14 May 2017 00:15:43 +0200 Subject: [PATCH 06/16] Fixed package.xml. * [fix] fixed wrong dependency to opencv (this fixes rosdep issues) * [mod] added dependency to irrlicht --- package.xml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index b9434fe..33f3eb3 100644 --- a/package.xml +++ b/package.xml @@ -11,12 +11,13 @@ catkin + libopencv-dev + libirrlicht-dev std_msgs geometry_msgs roscpp sensor_msgs nav_msgs - opencv2 tf visualization_msgs image_transport @@ -25,12 +26,13 @@ message_generation message_runtime + libopencv-dev + libirrlicht-dev std_msgs geometry_msgs roscpp sensor_msgs nav_msgs - opencv2 tf visualization_msgs image_transport From 92f8d1a43aabc004dc4f20681e71c595f8a35884 Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Sun, 14 May 2017 00:38:17 +0200 Subject: [PATCH 07/16] Further fixes in package.xml. * [mod] added missing dependency to OpenGL --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index 33f3eb3..c6e30bf 100644 --- a/package.xml +++ b/package.xml @@ -13,6 +13,7 @@ libopencv-dev libirrlicht-dev + opengl std_msgs geometry_msgs roscpp @@ -28,6 +29,7 @@ libopencv-dev libirrlicht-dev + opengl std_msgs geometry_msgs roscpp From 4f68852c6a3acb7c1977dc3498889d63d15ab7c6 Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Sun, 14 May 2017 14:04:19 +0200 Subject: [PATCH 08/16] Added readme. This closes #6. * [add] added readme in ROS doc style, including travis-ci build status --- README.md | 173 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 173 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000..01ef06d --- /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 +* Maintainer: Peter Rudolph +* Authors: Michael Milford and Gordon Wyeth (RatSLAM), David Ball and Scott Heath (RatSLAM C++), David Ball (OpenRatSLAM) +* License: GNU General Public License v3.0 +* Source: git https://github.com/sem23/ratslam.git (branch: ratslam_ros) + +## 1. External Documentation + +Further documentation can be found on ROS wrapper authors [git](https://github.com/davidmball/ratslam). + +## 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_footprint") +> 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. From 6ffb35655395b5b846102185f623aa425dccf55f Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Sun, 14 May 2017 16:33:20 +0200 Subject: [PATCH 09/16] CMakeLists.txt cleanup. * [mod] moved boost linking to ratslam library target in CMakeLists.txt * [mod] enabled Cxx11 support in CMakeLists.txt * [mod] added "src" as include directory in CMakeLists.txt --- CMakeLists.txt | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b8c614a..b2caf48 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,6 +3,8 @@ project(ratslam_ros) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake) +# Enable Cxx11 support +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -57,12 +59,16 @@ CATKIN_DEPENDS message_runtime geometry_msgs std_msgs ## Your package locations should be listed before other locations # include_directories(include) include_directories( + src ${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} +) # uncomment is you don't have irrlicht installed add_definitions("-DHAVE_IRRLICHT") @@ -74,7 +80,7 @@ target_link_libraries(ratslam_lv ${IRRLICHT_LIBRARIES} ${OPENGL_LIBRARIES} ${OpenCV_LIBRARIES} - ${Boost_LIBRARIES}) +) add_executable(ratslam_pc src/main_pc.cpp) target_link_libraries(ratslam_pc @@ -82,7 +88,7 @@ target_link_libraries(ratslam_pc ratslam ${IRRLICHT_LIBRARIES} ${OPENGL_LIBRARIES} - ${Boost_LIBRARIES}) +) add_executable(ratslam_em src/main_em.cpp) target_link_libraries(ratslam_em @@ -90,7 +96,7 @@ target_link_libraries(ratslam_em ratslam ${IRRLICHT_LIBRARIES} ${OPENGL_LIBRARIES} - ${Boost_LIBRARIES}) +) add_executable(ratslam_vo src/main_vo.cpp) target_link_libraries(ratslam_vo @@ -98,7 +104,8 @@ target_link_libraries(ratslam_vo ratslam ${IRRLICHT_LIBRARIES} ${OPENGL_LIBRARIES} - ${OpenCV_LIBRARIES}) + ${OpenCV_LIBRARIES} +) #config files for devel set(MEDIA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/src/media) #devel use the files in the source dir From e590d701e5fd99fa7d7799049de629f240cb32e6 Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Fri, 26 May 2017 16:37:03 +0200 Subject: [PATCH 10/16] Update regarding requested changes. * [mod] modified C++11 compiler support include in CMakeLists.txt * [mod] added David as maintainer in README.md * [mod] linked original paper in external doc. part of README.md * [mod] changed default base_frame param to "base_link" * [mod] set authors and maintainers in package.xml * [mod] increased version number in package.xml * [mod] backward compat with original examples (if topic_root is set, old topic names are assumed) --- CMakeLists.txt | 11 ++++++++++- README.md | 8 ++++---- package.xml | 9 +++++++-- src/main_em.cpp | 12 ++++++++++-- src/main_lv.cpp | 15 ++++++++++++--- src/main_pc.cpp | 9 ++++++++- src/main_vo.cpp | 17 +++++++++++++++-- 7 files changed, 66 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b2caf48..54eedbc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,16 @@ project(ratslam_ros) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake) # Enable Cxx11 support -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +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) diff --git a/README.md b/README.md index 01ef06d..3ddf1dc 100644 --- a/README.md +++ b/README.md @@ -17,14 +17,14 @@ OpenRatSLAM is based on the RatSLAM C++ library written by David Ball and Scott 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 -* Maintainer: Peter Rudolph -* Authors: Michael Milford and Gordon Wyeth (RatSLAM), David Ball and Scott Heath (RatSLAM C++), David Ball (OpenRatSLAM) +* 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 -Further documentation can be found on ROS wrapper authors [git](https://github.com/davidmball/ratslam). +[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 @@ -78,7 +78,7 @@ topic_root + "/ExperienceMap/PathToGoal" ([nav_msgs/Path](http://docs.ros.org/ap "odom_frame" (Default: "odom") > The name of the odometry frame. -"base_frame" (Default: "base_footprint") +"base_frame" (Default: "base_link") > The name of the base frame. "tf_update_rate" (Default: 20.0) diff --git a/package.xml b/package.xml index c6e30bf..e9651d2 100644 --- a/package.xml +++ b/package.xml @@ -1,10 +1,15 @@ 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 diff --git a/src/main_em.cpp b/src/main_em.cpp index d8e3c6e..9e51bda 100644 --- a/src/main_em.cpp +++ b/src/main_em.cpp @@ -79,7 +79,7 @@ using namespace ratslam; std::string map_frame = std::string("map"); std::string odom_frame = std::string("odom"); -std::string base_frame = std::string("base_footprint"); +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"); @@ -356,6 +356,7 @@ int main(int argc, char* argv[]) get_setting_child(ratslam_settings, settings, "ratslam", true); get_setting_child(general_settings, settings, "general", true); + // backward compatibility, namespace or private nodehandle should do it more ROS like get_setting_from_ptree(topic_root, general_settings, "topic_root", (std::string) ""); ros::NodeHandle node; @@ -375,6 +376,13 @@ int main(int argc, char* argv[]) 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()) + { + odom_topic = topic_root + "/odom"; + } + // create the experience map object em = new ratslam::ExperienceMap(ratslam_settings); @@ -391,7 +399,7 @@ int main(int argc, char* argv[]) pub_goal_path = node.advertise(topic_root + "/ExperienceMap/PathToGoal", 1); // subs - ros::Subscriber sub_odometry = node.subscribe("odom", 0, odo_callback, ros::VoidConstPtr(), + 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, action_callback, diff --git a/src/main_lv.cpp b/src/main_lv.cpp index a5ec52e..ba58ebe 100644 --- a/src/main_lv.cpp +++ b/src/main_lv.cpp @@ -137,11 +137,10 @@ int main(int argc, char* argv[]) read_ini(argv[1], settings); get_setting_child(general_settings, settings, "general", true); + // 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); - lv = new ratslam::LocalViewMatch(ratslam_settings); - // initialize ROS node ros::init(argc, argv, "RatSLAMViewTemplate"); @@ -153,6 +152,16 @@ int main(int argc, char* argv[]) 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); + // try to load lvm if (!load_lvm(lvm_file_path)) { @@ -164,7 +173,7 @@ int main(int argc, char* argv[]) // image transport image_transport::ImageTransport it(node); - image_transport::Subscriber sub = it.subscribe("image", 0, image_callback); + 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); diff --git a/src/main_pc.cpp b/src/main_pc.cpp index a381c83..f632c21 100644 --- a/src/main_pc.cpp +++ b/src/main_pc.cpp @@ -172,6 +172,13 @@ int main(int argc, char* argv[]) 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()) + { + odom_topic = topic_root + "/odom"; + } + // create PoseCellNetwork object pc = new ratslam::PosecellNetwork(ratslam_settings); @@ -185,7 +192,7 @@ int main(int argc, char* argv[]) pub_pc = node.advertise(topic_root + "/PoseCell/TopologicalAction", 0); // subs - ros::Subscriber sub_odometry = node.subscribe("odom", 0, odo_callback, ros::VoidConstPtr(), + 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, template_callback, diff --git a/src/main_vo.cpp b/src/main_vo.cpp index 8023a03..c3eacb5 100644 --- a/src/main_vo.cpp +++ b/src/main_vo.cpp @@ -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("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("image", 1, image_callback); + image_transport::Subscriber sub = it.subscribe(image_topic, 1, image_callback); ros::spin(); From 553fbf6461cbf9fe12f835a0c8887e68c3321b52 Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Sat, 27 May 2017 14:43:00 +0200 Subject: [PATCH 11/16] Added ROS kinetic to travis config. * [mod] added ROS kinetic to .travis.yml --- .travis.yml | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index f365953..acd8878 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,16 +11,26 @@ notifications: # - 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="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="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="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="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 +# - source ./travis.sh # Enable this when you have a package-local script From 10549b45bc28b69412d2b693796440026bd3eac5 Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Tue, 13 Jun 2017 19:43:08 +0200 Subject: [PATCH 12/16] Added simple rostest to check basic functionality. This closes #4. * [mod] added testing section to CMakeLists.txt * [add] added irat_aus.test rostest file * [add] added copy of irat_aus config with graphics disabled for testing --- CMakeLists.txt | 21 +++++++++++++- config/config_irataus_test.txt.in | 28 ++++++++++++++++++ tests/irat_aus.test | 47 +++++++++++++++++++++++++++++++ 3 files changed, 95 insertions(+), 1 deletion(-) create mode 100644 config/config_irataus_test.txt.in create mode 100644 tests/irat_aus.test diff --git a/CMakeLists.txt b/CMakeLists.txt index 54eedbc..f7df67c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -79,7 +79,7 @@ target_link_libraries(ratslam ${Boost_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) @@ -163,3 +163,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/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/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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 6268f2784fabf4e580593fae75b7a42f142b7ec5 Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Tue, 13 Jun 2017 19:59:43 +0200 Subject: [PATCH 13/16] Updated package.xml. * [mod] added missing dependencies to wget tool in package.xml --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index e9651d2..61cf791 100644 --- a/package.xml +++ b/package.xml @@ -31,6 +31,7 @@ boost message_generation message_runtime + wget libopencv-dev libirrlicht-dev @@ -47,6 +48,7 @@ boost message_generation message_runtime + wget From 867e3f4be22fdcb61caa427fd854ddd7f2e4805f Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Tue, 13 Jun 2017 20:04:32 +0200 Subject: [PATCH 14/16] Updated package.xml. * [mod] added missing dependencies to compressed_image_transport in package.xml --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index 61cf791..f556b0d 100644 --- a/package.xml +++ b/package.xml @@ -27,6 +27,7 @@ tf visualization_msgs image_transport + compressed_image_transport nav_msgs boost message_generation @@ -44,6 +45,7 @@ tf visualization_msgs image_transport + compressed_image_transport nav_msgs boost message_generation From eb0b281877c7d26c5fff0bdb18492853a8e37bf2 Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Thu, 15 Jun 2017 20:37:48 +0200 Subject: [PATCH 15/16] Changed build dependency from OpenCV2 to OpenCV3. * [mod] added args to OpenCV find_package in CMakeLists.txt * [mod] added build and run dependency to opencv3 in package.xml --- CMakeLists.txt | 2 +- package.xml | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f7df67c..0b068e7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ endif() ## 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) +find_package(OpenCV 3 REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS serialization) find_package(Irrlicht REQUIRED) diff --git a/package.xml b/package.xml index f556b0d..5b33dd3 100644 --- a/package.xml +++ b/package.xml @@ -19,6 +19,7 @@ libopencv-dev libirrlicht-dev opengl + opencv3 std_msgs geometry_msgs roscpp @@ -37,6 +38,7 @@ libopencv-dev libirrlicht-dev opengl + opencv3 std_msgs geometry_msgs roscpp From edc9f81b074dda5ddc3136dde466ed47a8a7d73f Mon Sep 17 00:00:00 2001 From: Rudolph Peter z003n7ht Date: Thu, 15 Jun 2017 21:23:51 +0200 Subject: [PATCH 16/16] fixup! Changed build dependency from OpenCV2 to OpenCV3. * [mod] removed version arg from OpenCV find_package in CMakeLists.txt * [mod] added build and run dependency to cv_bridge in package.xml --- CMakeLists.txt | 15 +++++++-------- package.xml | 6 ++---- 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0b068e7..a9a1172 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,8 +20,7 @@ endif() ## 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 3 REQUIRED) -include_directories(${OpenCV_INCLUDE_DIRS}) +find_package(OpenCV REQUIRED) find_package(Boost REQUIRED COMPONENTS serialization) find_package(Irrlicht REQUIRED) find_package(OpenGL REQUIRED) @@ -69,6 +68,7 @@ CATKIN_DEPENDS message_runtime geometry_msgs std_msgs # include_directories(include) include_directories( src + ${OpenCV_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) @@ -77,6 +77,7 @@ add_library(ratslam src/ratslam/experience_map.cpp src/ratslam/posecell_network. target_link_libraries(ratslam ${Boost_LIBRARIES} + ${OpenCV_LIBRARIES} ) # comment this out, if you don't have irrlicht installed @@ -84,36 +85,34 @@ add_definitions("-DHAVE_IRRLICHT") add_executable(ratslam_lv src/main_lv.cpp) target_link_libraries(ratslam_lv - ${catkin_LIBRARIES} 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} + ${catkin_LIBRARIES} ) add_executable(ratslam_em src/main_em.cpp) target_link_libraries(ratslam_em - ${catkin_LIBRARIES} ratslam ${IRRLICHT_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 diff --git a/package.xml b/package.xml index 5b33dd3..305b5bb 100644 --- a/package.xml +++ b/package.xml @@ -16,10 +16,9 @@ catkin - libopencv-dev libirrlicht-dev opengl - opencv3 + cv_bridge std_msgs geometry_msgs roscpp @@ -35,10 +34,9 @@ message_runtime wget - libopencv-dev libirrlicht-dev opengl - opencv3 + cv_bridge std_msgs geometry_msgs roscpp