diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..29c4936 Binary files /dev/null and b/.DS_Store differ diff --git a/Author.md b/Author.md new file mode 100644 index 0000000..c8af2d2 --- /dev/null +++ b/Author.md @@ -0,0 +1,8 @@ +Original Authors +---------------- + + * [Chris Dunkers](cmdunkers@cmu.edu) + * [Eric Danziger](ericdanziger@cmu.edu) + +Contributors +------------ diff --git a/Author.md~ b/Author.md~ new file mode 100644 index 0000000..c27606d --- /dev/null +++ b/Author.md~ @@ -0,0 +1,8 @@ +Original Authors +---------------- + + * [Chris Dunkers](cmdunkers@cmu.edu) + * [Eric Danziger]() + +Contributors +------------ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..2036145 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,176 @@ +cmake_minimum_required(VERSION 2.8.3) +project(perception_system) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + nav_msgs + oddbot_msgs + roscpp + sensor_msgs + std_msgs + tf + cv_bridge +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# nav_msgs# oddbot_msgs# sensor_msgs# std_msgs +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES trajectory_controller +# CATKIN_DEPENDS nav_msgs oddbot_msgs roscpp sensor_msgs std_msgs tf +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories(include + ${catkin_INCLUDE_DIRS} + #${OpenCV_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(trajectory_controller +# src/${PROJECT_NAME}/trajectory_controller.cpp +# ) + +## Declare a cpp executable +add_executable(perception_tracker src/perception_tracker.cpp src/bot.cpp) +#add_executable(local_controller src/local_controller.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(trajectory_controller_node trajectory_controller_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(perception_tracker + ${catkin_LIBRARIES} + #opencv_ocl + #opencv_core + #opencv_features2d + opencv_nonfree + #opencv_highgui + #${OpenCV_LIBRARIES} +) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS trajectory_controller trajectory_controller_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_trajectory_controller.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/README.md b/README.md new file mode 100644 index 0000000..a00b4a7 --- /dev/null +++ b/README.md @@ -0,0 +1,9 @@ +perception system +================== +#### + +### License +For full terms and conditions, see the [LICENSE](LICENSE) file. + +### Authors +See the [AUTHORS](AUTHORS.md) file for a full list of contributors diff --git a/README.md~ b/README.md~ new file mode 100644 index 0000000..e7f4039 --- /dev/null +++ b/README.md~ @@ -0,0 +1,16 @@ +trajectory_controller +================== +#### trajectory_controller +The nodes associated with the trajectory control and following + +### gloabl_controller +Node associated with following the global path + +### local_controller +Node associated with following the local path + +### License +For full terms and conditions, see the [LICENSE](LICENSE) file. + +### Authors +See the [AUTHORS](AUTHORS.md) file for a full list of contributors diff --git a/include/.DS_Store b/include/.DS_Store new file mode 100644 index 0000000..14309ed Binary files /dev/null and b/include/.DS_Store differ diff --git a/include/perception_system/bot.h b/include/perception_system/bot.h new file mode 100644 index 0000000..35150ec --- /dev/null +++ b/include/perception_system/bot.h @@ -0,0 +1,61 @@ +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#ifndef BOT_H +#define BOT_H + + +#include +#include +#include +#include +#include +#include +#include +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/core/core.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/nonfree/nonfree.hpp" +#include "opencv2/calib3d/calib3d.hpp" + +using namespace cv; + +class Bot +{ + public: + Bot(); + Bot(int); + Bot(Point uL, Point lR); + ~Bot(); + + void updateBoxPos(Mat image1, Mat image2, float*, float*, float*); + void findGoalPos(Mat image1, Mat image2, float*, float*, float*); + void updateBoxSize(Mat image); + bool unitTest(Mat image); + + private: + bool showImages; + Point upperLeft; + Point lowerRight; + bool inertialTracking; + Mat templateIm; + int updateTemplate; + + std::vector confirmedFeatures; + std::vector gradientHistory; + std::vector xHistory; + std::vector yHistory; + + void checkBounds(Mat image, Point* uL, Point* lR); + void getTemplateMatch(Mat image, Point* matchUL, Point* matchLR, Mat* templateImage, double* strongestResponse); + void matchPoints(std::vector* keypointsIn1, std::vector* keypointsIn2,Mat image1, Mat image2, std::vector* keypointsOut1, std::vector* keypointsOut2, int thresh); + + + +}; + + +#endif diff --git a/include/perception_system/bot.h~ b/include/perception_system/bot.h~ new file mode 100644 index 0000000..0f7a28b --- /dev/null +++ b/include/perception_system/bot.h~ @@ -0,0 +1,58 @@ +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#ifndef BOT_H +#define BOT_H + + +#include +#include +#include +#include +#include +#include +#include +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/core/core.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/nonfree/nonfree.hpp" +#include "opencv2/calib3d/calib3d.hpp" + +using namespace cv; + +class Bot +{ + public: + Bot(Point uL, Point lR); + ~Bot(); + + void updateBoxPos(Mat image1, Mat image2); + void findGoalPos(Mat image1, Mat image2); + void updateBoxSize(Mat image); + bool unitTest(Mat image); + + private: + Point upperLeft; + Point lowerRight; + bool inertialTracking; + Mat templateIm; + int updateTemplate; + + std::vector confirmedFeatures; + std::vector gradientHistory; + std::vector xHistory; + std::vector yHistory; + + void checkBounds(Mat image, Point* uL, Point* lR); + void getTemplateMatch(Mat image, Point* matchUL, Point* matchLR, Mat* templateImage, double* strongestResponse); + void matchPoints(std::vector* keypointsIn1, std::vector* keypointsIn2,Mat image1, Mat image2, std::vector* keypointsOut1, std::vector* keypointsOut2, int thresh); + + + +}; + + +#endif diff --git a/include/perception_system/global_controller.h b/include/perception_system/global_controller.h new file mode 100644 index 0000000..ce82b94 --- /dev/null +++ b/include/perception_system/global_controller.h @@ -0,0 +1,44 @@ +/* + * Global controller to follow a trajectory + * + * By: Christopher Dunkers, cmdunkers@cmu.edu + * March 1, 2015 + */ + +#ifndef GLOBAL_CONTROLLER_H_ +#define GLOBAL_CONTROLLER_H_ + +#include +#include "nav_msgs/Path.h" +#include +#include +#include + +class global_controller { + + public: + global_controller(); + + private: + void create_path(); + void update_waypoints(const nav_msgs::Path::ConstPtr& msg); + nav_msgs::Path goal_waypoints; + tf::TransformListener tf_listener; + + double step; + geometry_msgs::PoseStamped cur_robot_pose; + nav_msgs::Path plan; + + //alphas start with the constant term and increase + //ie a[0] + a[1]*t + a[2]*t^2 ... + std::vector > x_alphas; + std::vector > y_alphas; + std::vector > h_alphas; + + ros::Subscriber waypoints_sub; + ros::Publisher plan_pub; + +}; + + +#endif diff --git a/include/perception_system/local_controller.h b/include/perception_system/local_controller.h new file mode 100644 index 0000000..8028861 --- /dev/null +++ b/include/perception_system/local_controller.h @@ -0,0 +1,42 @@ +/* + * Local controller to follow a trajectory + * + * By: Eric Danziger + * March 1, 2015 + */ + +#ifndef LOCAL_CONTROLLER_H_ +#define LOCAL_CONTROLLER_H_ + +// ROS includes. +#include "ros/ros.h" +#include "ros/time.h" +#include "std_msgs/Float32.h" +#include "geometry_msgs/Twist.h" +#include "tf/transform_broadcaster.h" +#include "nav_msgs/Odometry.h" +#include "nav_msgs/Path.h" +#include "tf/LinearMath/Transform.h" + +#include "oddbot_msgs/MotorCommand.h" +#include + + + +class localController +{ + public: + localController(); + ~localController(); + void publishMessage(ros::Publisher *pub_message); + void pathMessageCallback(const nav_msgs::Path::ConstPtr &msg); + void odomUpdateCallback(const nav_msgs::Odometry::ConstPtr &msg); + private: + nav_msgs::Path currentPath; + int pathCounter; + geometry_msgs::Twist currentVel; + geometry_msgs::Pose currentPose; +}; + + +#endif diff --git a/include/perception_system/perception_tracker.h b/include/perception_system/perception_tracker.h new file mode 100644 index 0000000..11c0618 --- /dev/null +++ b/include/perception_system/perception_tracker.h @@ -0,0 +1,54 @@ +/* + * Perception tracker finds robot and sends pose in world + * + * By: Eric Danziger + * March 1, 2015 + */ + +#ifndef PERCEPTION_TRACKER_H_ +#define PERCEPTION_TRACKER_H_ + +// ROS includes. +#include "ros/ros.h" +#include "ros/time.h" +#include "std_msgs/Float32.h" +#include "geometry_msgs/Twist.h" +#include "geometry_msgs/Pose2D.h" +#include "tf/transform_broadcaster.h" +#include "nav_msgs/Odometry.h" +#include "nav_msgs/Path.h" +#include "tf/LinearMath/Transform.h" +#include "perception_system/bot.h" +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/core/core.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/nonfree/nonfree.hpp" +#include "opencv2/calib3d/calib3d.hpp" + +//#include "oddbot_msgs/MotorCommand.h" +#include + + + +class perceptionTracker +{ +public: +perceptionTracker(); +~perceptionTracker(); +void publishMessage(ros::Publisher *pub_message); +geometry_msgs::Pose2D robotPose; +geometry_msgs::Pose2D goalPose; + + +private: +nav_msgs::Path currentPath; +int pathCounter; +geometry_msgs::Twist currentVel; +geometry_msgs::Pose currentPose; + +}; + + +#endif diff --git a/include/perception_system/perception_tracker.h~ b/include/perception_system/perception_tracker.h~ new file mode 100644 index 0000000..8028861 --- /dev/null +++ b/include/perception_system/perception_tracker.h~ @@ -0,0 +1,42 @@ +/* + * Local controller to follow a trajectory + * + * By: Eric Danziger + * March 1, 2015 + */ + +#ifndef LOCAL_CONTROLLER_H_ +#define LOCAL_CONTROLLER_H_ + +// ROS includes. +#include "ros/ros.h" +#include "ros/time.h" +#include "std_msgs/Float32.h" +#include "geometry_msgs/Twist.h" +#include "tf/transform_broadcaster.h" +#include "nav_msgs/Odometry.h" +#include "nav_msgs/Path.h" +#include "tf/LinearMath/Transform.h" + +#include "oddbot_msgs/MotorCommand.h" +#include + + + +class localController +{ + public: + localController(); + ~localController(); + void publishMessage(ros::Publisher *pub_message); + void pathMessageCallback(const nav_msgs::Path::ConstPtr &msg); + void odomUpdateCallback(const nav_msgs::Odometry::ConstPtr &msg); + private: + nav_msgs::Path currentPath; + int pathCounter; + geometry_msgs::Twist currentVel; + geometry_msgs::Pose currentPose; +}; + + +#endif diff --git a/include/trajectory_controller/global_controller.h b/include/trajectory_controller/global_controller.h new file mode 100644 index 0000000..ce82b94 --- /dev/null +++ b/include/trajectory_controller/global_controller.h @@ -0,0 +1,44 @@ +/* + * Global controller to follow a trajectory + * + * By: Christopher Dunkers, cmdunkers@cmu.edu + * March 1, 2015 + */ + +#ifndef GLOBAL_CONTROLLER_H_ +#define GLOBAL_CONTROLLER_H_ + +#include +#include "nav_msgs/Path.h" +#include +#include +#include + +class global_controller { + + public: + global_controller(); + + private: + void create_path(); + void update_waypoints(const nav_msgs::Path::ConstPtr& msg); + nav_msgs::Path goal_waypoints; + tf::TransformListener tf_listener; + + double step; + geometry_msgs::PoseStamped cur_robot_pose; + nav_msgs::Path plan; + + //alphas start with the constant term and increase + //ie a[0] + a[1]*t + a[2]*t^2 ... + std::vector > x_alphas; + std::vector > y_alphas; + std::vector > h_alphas; + + ros::Subscriber waypoints_sub; + ros::Publisher plan_pub; + +}; + + +#endif diff --git a/include/trajectory_controller/local_controller.h b/include/trajectory_controller/local_controller.h new file mode 100644 index 0000000..8028861 --- /dev/null +++ b/include/trajectory_controller/local_controller.h @@ -0,0 +1,42 @@ +/* + * Local controller to follow a trajectory + * + * By: Eric Danziger + * March 1, 2015 + */ + +#ifndef LOCAL_CONTROLLER_H_ +#define LOCAL_CONTROLLER_H_ + +// ROS includes. +#include "ros/ros.h" +#include "ros/time.h" +#include "std_msgs/Float32.h" +#include "geometry_msgs/Twist.h" +#include "tf/transform_broadcaster.h" +#include "nav_msgs/Odometry.h" +#include "nav_msgs/Path.h" +#include "tf/LinearMath/Transform.h" + +#include "oddbot_msgs/MotorCommand.h" +#include + + + +class localController +{ + public: + localController(); + ~localController(); + void publishMessage(ros::Publisher *pub_message); + void pathMessageCallback(const nav_msgs::Path::ConstPtr &msg); + void odomUpdateCallback(const nav_msgs::Odometry::ConstPtr &msg); + private: + nav_msgs::Path currentPath; + int pathCounter; + geometry_msgs::Twist currentVel; + geometry_msgs::Pose currentPose; +}; + + +#endif diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..aedd9d5 --- /dev/null +++ b/package.xml @@ -0,0 +1,67 @@ + + + perception_system + 0.0.0 + The perception tracker system package + + + + + cmdunkers + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + nav_msgs + oddbot_msgs + roscpp + sensor_msgs + std_msgs + tf + cv_bridge + nav_msgs + oddbot_msgs + roscpp + sensor_msgs + std_msgs + tf + cv_bridge + + + + + + + + + + + diff --git a/src/.DS_Store b/src/.DS_Store new file mode 100644 index 0000000..10c8023 Binary files /dev/null and b/src/.DS_Store differ diff --git a/src/20APRprojmats.yml b/src/20APRprojmats.yml new file mode 100644 index 0000000..c44ff3f --- /dev/null +++ b/src/20APRprojmats.yml @@ -0,0 +1,17 @@ +%YAML:1.0 + P1: !!opencv-matrix + rows: 3 + cols: 4 + dt: f + data: [ 1051.599302, 0.000000, 0.000000, + 0.000000, 0.000000, 982.327212, 0.000000, + 0.000000, 616.785732, 317.302023, 1.000000, + 0.000000] + P2: !!opencv-matrix + rows: 3 + cols: 4 + dt: f + data: [ 1037.902286, -18.161263, 58.993424, + -240585.567377, 18.182419, 971.813640, -20.717757, + -3090.051350, 647.716689, 342.365084, 29.903539, + -149735.483954] diff --git a/src/Makefile b/src/Makefile new file mode 100644 index 0000000..7a4d323 --- /dev/null +++ b/src/Makefile @@ -0,0 +1,32 @@ +CC = g++ +LINK = g++ +INSTALL = install +CFLAGS = -Wall `pkg-config opencv --cflags` -I /usr/include/boost-1_46 -I. +LFLAGS = -Wall `pkg-config opencv --libs` -L /usr/lib -lboost_system -lboost_filesystem -lopencv_features2d -lopencv_nonfree -lopencv_ocl -lopencv_highgui -lopencv_core -lpthread +#OBJS = car.o cartracker.o + +all: bottracker + + +bot_tracker.o: bot_tracker.cpp bot.h + $(CC) $(CFLAGS) -c $^ + +bot.o: bot.h + + +bottracker: bot_tracker.o bot.o + $(LINK) $^ -o $@ $(LFLAGS) + +#test.o: test.cpp car.h +# $(CC) $(CFLAGS) -c $^ + +#test: test.o car.o +# $(LINK) $^ -o $@ $(LFLAGS) + + +clean: + rm -f *.o *.gch bottracker* + +install: + cp bottracker /usr/bin/ + diff --git a/src/Makefile~ b/src/Makefile~ new file mode 100644 index 0000000..d5ec579 --- /dev/null +++ b/src/Makefile~ @@ -0,0 +1,33 @@ +CC = g++ +LINK = g++ +INSTALL = install +CFLAGS = -Wall `pkg-config opencv --cflags` -I /usr/include/boost-1_46 -I. +LFLAGS = -Wall `pkg-config opencv --libs` -L /usr/lib -lboost_system -lboost_filesystem -lopencv_features2d -lopencv_nonfree -lopencv_ocl -lopencv_highgui -lopencv_core -lpthread +#OBJS = car.o cartracker.o + +all: cartracker test + + + +cartracker.o: cartracker.cpp car.h + $(CC) $(CFLAGS) -c $^ + +car.o: car.h + + +cartracker: cartracker.o car.o + $(LINK) $^ -o $@ $(LFLAGS) + +test.o: test.cpp car.h + $(CC) $(CFLAGS) -c $^ + +test: test.o car.o + $(LINK) $^ -o $@ $(LFLAGS) + + +clean: + rm -f *.o *.gch + +install: + cp cartracker /usr/bin/ + diff --git a/src/OP.cpp b/src/OP.cpp new file mode 100644 index 0000000..739fb99 --- /dev/null +++ b/src/OP.cpp @@ -0,0 +1,93 @@ +solveOP(double * alphas, int N, int r, double * pos, double * vel, double * acc, double * jerk, double * snap){ + + //Add in checks + + num_polys + for(int poly = 0; poly < num_polys; poly++){ + //compute Hessian + + + } + + + +} + +solveOP(double * alphas, int N, double * pos, double * vel, double * acc, double * jerk, double * snap){ + int r = 4; + + //Add in checks + + num_polys = + A_seg + + +} + +solveOP(double * alphas, int N, double * pos, double * vel, double * acc){ + int r = 2; + + +} + +solveOP(double * alphas, int N, double * pos){ + int r = 0; + + +} + +calcA(double * A[], double t, int r, int N){ + + int s = 1; + for(int n = 0; n < N + 1; n++){ + if(n-1 >= r){ + for(int m = 0; m < r; m++){ + s = s * (n-1-m); + } + A(n) = s * pow(t,n-1-r); + } else { + A(n) = 0; + } + s = 1; + } +} + +calcAMatrix(double * A[], int r, double t, bool isLast){ + + for(int i = 0; i < r+1; i++){ + calcA(A0,0,i,N); + calcA(At,t,i,N); + } + + if(isLast) + A = [A0; At]; + else + A = [A0; -1*At]; + +} + +computeHessian(double * Q[], int N, int r, double t){ + for(int i = 0; i < N + 1; i++){ + for(int l = 0; l > N + 1; l++){ + if(i >= r && l >=r){ + for(int m = 0, m < r; m++){ + s = s * ((i-m)*(l-m)); + } + Q(i,l) = 2*s*(pow(t,i+l-2*r)/(i+l-(2*r)+1)); + } else { + Q(i,l) = 0; + } + s = 1; + } + } +} + +double getValueFromPolynomial(std::vector alphas, double time){ + + double value = 0.0; + for(int i = 0; i < size(alphas); i++){ + value += alphas[i] * pow(t,i); + } + + return value; +} diff --git a/src/bot.cpp b/src/bot.cpp new file mode 100644 index 0000000..8b4774b --- /dev/null +++ b/src/bot.cpp @@ -0,0 +1,820 @@ +//This is the Car class +//The main focus is on two public functions to update the position and size of the tracking box +//The updateBoxSize function can be modified to be more robust for other image sets +// +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#include "perception_system/bot.h" +using namespace cv; + +Bot::Bot() +{ + upperLeft.x = 1; + upperLeft.y = 1; + lowerRight.x = 100; + lowerRight.y = 100; + showImages = false; + //checkBounds(initialIm, &upperLeft, &lowerRight); + //templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + updateTemplate = 1; +} + + +Bot::Bot(int sI) +{ + if(sI == 1) + showImages = true; + else + showImages = false; + upperLeft.x = 1; + upperLeft.y = 1; + lowerRight.x = 100; + lowerRight.y = 100; + inertialTracking = false; + //checkBounds(initialIm, &upperLeft, &lowerRight); + //templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + updateTemplate = 1; +} + +Bot::Bot(Point uL, Point lR) +{ + upperLeft = uL; + lowerRight = lR; + inertialTracking = false; + //checkBounds(initialIm, &upperLeft, &lowerRight); + //templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + updateTemplate = 1; +} + +Bot::~Bot() +{ +} + +void Bot::matchPoints(std::vector* keypointsIn1, std::vector* keypointsIn2,Mat image1, Mat image2, std::vector* keypointsOut1, std::vector* keypointsOut2, int thresh) +{ + if(!keypointsIn1->size() || !keypointsIn2->size()) + { + //-- Detect the keypoints using SURF Detector + int minHessian = 400; + + SurfFeatureDetector detector( minHessian ); + + detector.detect( image1, *keypointsIn1 ); + detector.detect( image2, *keypointsIn2 ); + } + + //-- Extract features for matching + SurfDescriptorExtractor extractor; + + Mat descriptors1, descriptors2; + + extractor.compute(image1, *keypointsIn1, descriptors1); + extractor.compute(image2, *keypointsIn2, descriptors2); + + FlannBasedMatcher matcher; + std::vector matches; + + //-- Match the car SURF points with all the next images SURF points + matcher.match(descriptors1,descriptors2, matches); + + //-- Find match strengths + double maxDist = 0; double minDist = 100; + + for( int i = 0; i < descriptors1.rows; i++ ) + { double dist = matches[i].distance; + if( dist < minDist ) minDist = dist; + if( dist > maxDist ) maxDist = dist; + } + + //-- Keep good matches + std::vector< DMatch > goodMatches; + + for( int i = 0; i < descriptors1.rows; i++ ) + { if( matches[i].distance <= max(thresh*minDist, 0.02) ) + { goodMatches.push_back( matches[i]); } + } + + //-- Fill the good matches keypoint vectors + for ( int i = 0; i < goodMatches.size(); i++) + { + (*keypointsOut1).push_back((*keypointsIn1)[goodMatches[i].queryIdx]); + (*keypointsOut2).push_back((*keypointsIn2)[goodMatches[i].trainIdx]); + } + +} + +void Bot::findGoalPos(Mat imageL, Mat imageR, float *xGoal, float *yGoal, float *tGoal) +{ + double lBestResponse=100; + int lMostPoints=0; + int lRotI; + float lBestDeg, lScale; + Point lBestUL,lBestLR; + + double rBestResponse=100; + int rRotI; + float rBestDeg, rScale; + Point rBestUL,rBestLR; + + //-- Check left image for goal + + for (float scale=.7;scale < 2.00; scale=scale+.10) + { + Point locUL,locLR; + double sResponse; + float deg; + char imName[30]; + Mat tempIm,tempEdges,lTempEdges; + Size scaleSize; + sprintf(imName,"images/templates/20APR/leftGoal.jpg"); + //printf("%s %f \n",imName,scale); + tempIm = imread(imName, CV_LOAD_IMAGE_COLOR); + scaleSize.height = tempIm.rows*scale; + scaleSize.width = tempIm.cols*scale; + resize(tempIm,tempIm,scaleSize); + + Mat halfImage = imageL(Range(imageL.rows/2,imageL.rows),Range(1,imageL.cols)); + if(showImages) + { + imshow("goalTemplate",tempIm); + moveWindow("goalTemplate",10,500); + //imshow("edges",lTempEdges); + waitKey(10); + } + + getTemplateMatch(imageL,&locUL,&locLR, &tempIm, &sResponse); + deg = 0; + + Mat imageS = imageL.clone(); + rectangle(imageS,locUL,locLR,10,3); + //sResponse = sResponse/(8900*pow(scale,2)); + if (lBestResponse < sResponse) + { + lScale = scale; + lRotI = 1; + lBestUL = locUL; + lBestLR = locLR; + lBestResponse = sResponse; + lBestDeg = deg; + printf("lBest Response: %f\n",lBestResponse); + } + if(showImages) + { + rectangle(imageS,lBestUL,lBestLR,205,3); + imshow("Left Image Goal",imageS); + moveWindow("Left Image Goal",200,400); + waitKey(10); + } + } + + + //-- Check right image for goal + for (float scale=.7;scale < 2.00; scale=scale+.10) + { + Point locUL,locLR; + double sResponse; + float deg; + char imName[30]; + Mat tempIm,tempEdges,rTempEdges; + Size scaleSize; + sprintf(imName,"images/templates/20APR/rightGoal.jpg"); + //printf("%s %f \n",imName,scale); + tempIm = imread(imName, CV_LOAD_IMAGE_COLOR); + scaleSize.height = tempIm.rows*scale; + scaleSize.width = tempIm.cols*scale; + + resize(tempIm,tempIm,scaleSize); + + Mat halfImage = imageR(Range(imageR.rows/2,imageR.rows),Range(1,imageR.cols)); + if(showImages) + { + imshow("goalTemplate",tempIm); + //waitKey(10); + } + getTemplateMatch(imageR,&locUL,&locLR, &tempIm, &sResponse); + deg = 0; + //printf("LEFT\ndegree: %f \ntemplate: %d \nlocUL-x: %d \nlocUL-y: %d \nlocLR-x: %d\nlocLR-y: %d\n",deg,i,locUL.x,locUL.y,locLR.x,locLR.y); + Mat imageS = imageR.clone(); + rectangle(imageS,locUL,locLR,10,3); + //sResponse = sResponse/(8900*pow(scale,2)); + if (rBestResponse < sResponse) + { + rScale = scale; + rRotI = 1; + rBestUL = locUL; + rBestLR = locLR; + rBestResponse = sResponse; + rBestDeg = deg; + printf("rBest Response: %f\n",rBestResponse); + } + if(showImages) + { + rectangle(imageS,rBestUL,rBestLR,200,3); + imshow("Right Image Goal",imageS); + moveWindow("Right Image Goal",200,500); + } + //rectangle(imgKeypoints1,upperLeft,lowerRight,10,3); + waitKey(10); + } + + if(lBestDeg == 0 && rBestDeg > 180) + { + lBestDeg = 360.0; + } + if(rBestDeg == 0 && lBestDeg > 180) + { + rBestDeg = 360.0; + } + + float bestDeg = (lBestDeg + rBestDeg)/2 + 45.0; + if (bestDeg > 360) + { + bestDeg = bestDeg - 360.0; + } + printf("\nScale: %f\n",(lScale + rScale)/2); + + printf("leftLoc %d %d\n",(lBestUL.x + lBestLR.x)/2,(lBestUL.y + lBestLR.y)/2); + printf("rightLoc %d %d\n",(rBestUL.x + rBestLR.x)/2,(rBestUL.y + rBestLR.y)/2); + + + printf("\nBest Estimate of Rotation: %f\n",bestDeg); + + //-- Translate scale and position + float yTrans = 5.8365 - 3.3654*(lScale + rScale)/2; + float xLeftTrans = ((lBestUL.x + lBestLR.x)/2 - 250)*(4.25/(1100-250)); + float xRightTrans = ((rBestUL.x + rBestLR.x)/2 - 150)*(4.25/(1000-150)); + + *yGoal = yTrans; + *xGoal = (xLeftTrans+xRightTrans)/2; + *tGoal = bestDeg; + + printf("Estimate of X,Y pos of GOAL: %f %f\n",(xLeftTrans+xRightTrans)/2,yTrans); + + + waitKey(1); + +} + +void Bot::updateBoxPos(Mat imageL, Mat imageR, float *xBot, float *yBot, float *tBot) +{ + double lBestResponse=100; + int lMostPoints=0; + int lRotI; + float lBestDeg, lScale; + Point lBestUL,lBestLR; + + double rBestResponse=100; + int rRotI; + float rBestDeg, rScale; + Point rBestUL,rBestLR; + + //-- Check left image for robot + for (int i=1;i<9;i++) + { + for (float scale=.6;scale < 2.00; scale=scale+.15) + { + Point locUL,locLR; + double sResponse; + float deg; + char imName[30]; + Mat tempIm,tempEdges,lTempEdges; + Size scaleSize; + sprintf(imName,"images/templates/20APR/left%03d.jpg",i); + //printf("%s %f \n",imName,scale); + tempIm = imread(imName, CV_LOAD_IMAGE_COLOR); + scaleSize.height = tempIm.rows*scale; + scaleSize.width = tempIm.cols*scale; + + //printf("scale height %d width %d\n",scaleSize.height,scaleSize.width); + sprintf(imName,"%f d",(i*360.0/8)); + + resize(tempIm,tempIm,scaleSize); + + //Mat halfImage = lTempEdges(Range(imageL.cols/2,imageL.cols),Range(1,imageL.rows)); + Mat halfImage = imageL(Range(imageL.rows/2,imageL.rows),Range(1,imageL.cols)); + + //templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + if(showImages) + { + imshow("template",tempIm); + moveWindow("template",10,10); + //imshow("edges",lTempEdges); + waitKey(1); + } + + getTemplateMatch(halfImage,&locUL,&locLR, &tempIm, &sResponse); + deg = (i-1)*(360.0/8); + //printf("LEFT\ndegree: %f \ntemplate: %d \nlocUL-x: %d \nlocUL-y: %d \nlocLR-x: %d\nlocLR-y: %d\n",deg,i,locUL.x,locUL.y,locLR.x,locLR.y); + + Mat imageS = halfImage.clone(); + rectangle(imageS,locUL,locLR,10,3); + //sResponse = sResponse/(8900*pow(scale,2)); + if (lBestResponse < sResponse) + { + lScale = scale; + lRotI = i; + lBestUL = locUL; + lBestLR = locLR; + lBestResponse = sResponse; + lBestDeg = deg; + //printf("lBest Response: %f\n",lBestResponse); + } + if(showImages) + { + rectangle(imageS,lBestUL,lBestLR,205,3); + imshow("Left Image",imageS); + moveWindow("Left Image",300,10); + } + + waitKey(1); + } + } + + printf("\nLBest Estimate of Left Rotation: %f\n",lBestDeg); + + //-- Check right image for robot + for (int i=1;i<9;i++) + { + for (float scale=.6;scale < 2.00; scale=scale+.15) + { + Point locUL,locLR; + double sResponse; + float deg; + char imName[30]; + Mat tempIm,tempEdges,rTempEdges; + Size scaleSize; + sprintf(imName,"images/templates/20APR/right%03d.jpg",i); + //printf("%s %f \n",imName,scale); + tempIm = imread(imName, CV_LOAD_IMAGE_COLOR); + scaleSize.height = tempIm.rows*scale; + scaleSize.width = tempIm.cols*scale; + + // printf("scale height %d width %d\n",scaleSize.height,scaleSize.width); + sprintf(imName,"%f d",(i*360.0/8)); + + resize(tempIm,tempIm,scaleSize); + + Mat halfImage = imageR(Range(imageR.rows/2,imageR.rows),Range(1,imageR.cols)); + if(showImages) + { + imshow("template",tempIm); + moveWindow("template",10,10); + waitKey(1); + } + getTemplateMatch(halfImage,&locUL,&locLR, &tempIm, &sResponse); + deg = (i-1)*(360.0/8); + //printf("LEFT\ndegree: %f \ntemplate: %d \nlocUL-x: %d \nlocUL-y: %d \nlocLR-x: %d\nlocLR-y: %d\n",deg,i,locUL.x,locUL.y,locLR.x,locLR.y); + Mat imageS = halfImage.clone(); + rectangle(imageS,locUL,locLR,10,3); + //sResponse = sResponse/(8900*pow(scale,2)); + if (rBestResponse < sResponse) + { + rScale = scale; + rRotI = i; + rBestUL = locUL; + rBestLR = locLR; + rBestResponse = sResponse; + rBestDeg = deg; + //printf("rBest Response: %f\n",rBestResponse); + } + if(showImages) + { + rectangle(imageS,rBestUL,rBestLR,200,3); + imshow("Right Image",imageS); + moveWindow("Right Image",300,250); + + waitKey(1); + } + } + } + + printf("\nBest Estimate of Right Rotation: %f\n",rBestDeg); + + //-- Estimate a box + + + //-- Map it to something based on the boundary values + if(lBestDeg == 0 && rBestDeg > 180) + { + lBestDeg = 360.0; + } + if(rBestDeg == 0 && lBestDeg > 180) + { + rBestDeg = 360.0; + } + + float bestDeg = (lBestDeg + rBestDeg)/2;// + 45.0; + if (bestDeg > 360) + { + bestDeg = bestDeg - 360.0; + } + printf("\nScale: %f\n",(lScale + rScale)/2); + + printf("leftLoc %d %d\n",(lBestUL.x + lBestLR.x)/2,(lBestUL.y + lBestLR.y)/2); + printf("rightLoc %d %d\n",(rBestUL.x + rBestLR.x)/2,(rBestUL.y + rBestLR.y)/2); + + + printf("\nBest Estimate of Rotation: %f\n",bestDeg); + + //-- Translate scale and position + float yTrans = 5.8365 - 3.3654*(lScale + rScale)/2; + float xLeftTrans = ((lBestUL.x + lBestLR.x)/2 - 250)*(4.25/(1100-250)); + float xRightTrans = ((rBestUL.x + rBestLR.x)/2 - 150)*(4.25/(1000-150)); + + *yBot = yTrans; + *xBot = (xLeftTrans+xRightTrans)/2; + *tBot = bestDeg; + + + printf("Estimate of X,Y pos: %f %f\n",(xLeftTrans+xRightTrans)/2,yTrans); + + waitKey(10); +} + + //-- Match points inside the boxes in the Left and Right images + // int minHessian = 400; + // SurfFeatureDetector detector( minHessian ); + + // std::vector keypointsL, keypointsR, keypointsBotL, keypointsBotR, keypointsGML, keypointsGMR; + + // detector.detect( imageL, keypointsL ); + // detector.detect( imageR, keypointsR ); + // Mat imgKeypointsL; Mat imgKeypointsR; + + // //-- Detect keypoints that are within the box -LEFT + + // Mat halfImage = imageR(Range(imageR.rows/2,imageR.rows),Range(1,imageR.cols)); + + + // for (int i = 0; i < keypointsL.size(); i++) + // { + // if (keypointsL[i].pt.x < lBestLR.x && keypointsL[i].pt.x > lBestUL.x && + // keypointsL[i].pt.y < (lBestLR.y + imageL.rows/2) && keypointsL[i].pt.y > (lBestUL.y + imageL.rows/2)) + // { + // keypointsBotL.push_back(keypointsL[i]); + // } + // } + // //-- Detect keypoints that are within the box - RIGHT + + // for (int i = 0; i < keypointsR.size(); i++) + // { + // if (keypointsR[i].pt.x < rBestLR.x && keypointsR[i].pt.x > rBestUL.x && + // keypointsR[i].pt.y < (rBestLR.y + imageR.rows/2) && keypointsR[i].pt.y > (rBestUL.y + imageR.rows/2)) + // { + // keypointsBotR.push_back(keypointsR[i]); + // } + // } + //-- Match points from within the box to the next frame + // matchPoints(&keypointsBotL,&keypointsBotR,imageL,imageR,&keypointsGML,&keypointsGMR,5.1); + + + //-- Triangulate distance + + // Mat lProj,rProj; + // string paramsyml = "projmats.yml"; + // FileStorage fsP(paramsyml,FileStorage::READ); + // fsP["P1"] >>lProj; + // fsP["P2"] >>rProj; + + // // std::cout<<"Printing contents of proj mats :"< stLP,stRP; + // // string paramsyml = "standardPoints.yml"; + // // FileStorage fsP(paramsyml,FileStorage::READ); + // // fsP["leftP"] >>stLP; + // // fsP["rightP"] >>stRP; + + // Mat worldPoints,imageWithKeyPointsL,imageWithKeyPointsR; + // std::vector lPoints,rPoints; + // for (int i = 0; i < keypointsGML.size(); i++) + // { + // lPoints.push_back(keypointsGML[i].pt); + // } + // for (int i = 0; i < keypointsGMR.size(); i++) + // { + // rPoints.push_back(keypointsGMR[i].pt); + // } + // //drawKeypoints( imageL, keypointsGML, imageWithKeyPointsL, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); + // //drawKeypoints( imageR, keypointsGMR, imageWithKeyPointsR, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); + + // //imshow("With Keypoints L", imageWithKeyPointsL); + // //imshow("With Keypoints R", imageWithKeyPointsR); + // waitKey(10); + + // Point2f tempPoint; + // // //left 0,0 + // tempPoint.x = 206; + // tempPoint.y = 568; + // lPoints.push_back(tempPoint); + // // //left 0,10 + // tempPoint.x = 252; + // tempPoint.y = 409; + // lPoints.push_back(tempPoint); + // // //left 10,0 + // tempPoint.x = 1134; + // tempPoint.y = 502; + // lPoints.push_back(tempPoint); + // // //left 10,10 + // tempPoint.x = 762; + // tempPoint.y = 395; + // lPoints.push_back(tempPoint); + + // // //right 0,0 + // tempPoint.x = 71; + // tempPoint.y = 636; + // rPoints.push_back(tempPoint); + // // //right 0,10 + // tempPoint.x = 169; + // tempPoint.y = 474; + // rPoints.push_back(tempPoint); + // //right 10,0 + // tempPoint.x = 1018; + // tempPoint.y = 545; + // rPoints.push_back(tempPoint); + // // //right 10,10 + // tempPoint.x = 689; + // tempPoint.y = 449; + // rPoints.push_back(tempPoint); + + // // //std::cout << "\n2D POINTS" << std::endl << lPoints << std::endl << rPoints < x,y,z; + // for (int i = 0; i < (worldPoints.cols - 4); i++) + // { + // worldPoints.at(0,i) /= worldPoints.at(3,i); + // x.push_back(worldPoints.at(0,i)); + // worldPoints.at(1,i) /= worldPoints.at(3,i); + // y.push_back(worldPoints.at(1,i)); + // worldPoints.at(2,i) /= worldPoints.at(3,i); + // z.push_back(worldPoints.at(2,i)); + // worldPoints.at(3,i) /= worldPoints.at(3,i); + // } + + // float avgX = std::accumulate(x.begin(), x.end(), 0) / (float)x.size(); + // float avgY = std::accumulate(y.begin(), y.end(), 0) / (float)y.size(); + // float avgZ = std::accumulate(z.begin(), z.end(), 0) / (float)z.size(); + + // //std::cout<< "\nworldPoints: "<< std::endl << worldPoints << std::endl; + // // // printf("WorldPoints: \n",worldPoints); + // printf("Avg X: %f\n",avgX); + // printf("Avg Y: %f\n",avgY); + // printf("Avg Z: %f\n",avgZ); + + // for (int i = (worldPoints.cols - 4); i < worldPoints.cols; i++) + // { + // //printf("i: %d\n",i); + // worldPoints.at(0,i) /= worldPoints.at(3,i); + // // //printf("X: %f\n",worldPoints.at(0,i)); + // worldPoints.at(1,i) /= worldPoints.at(3,i); + // // //printf("Y: %f\n",worldPoints.at(1,i)); + // worldPoints.at(2,i) /= worldPoints.at(3,i); + // // // printf("Z: %f\n",worldPoints.at(2,i)); + // worldPoints.at(3,i) /= worldPoints.at(3,i); + // } + + // Mat boundW = worldPoints(Range(0,(worldPoints.rows-1)),Range(worldPoints.cols-4,worldPoints.cols)); + + // std::cout << std::endl << boundW << std::endl; + + + + // int minHessian = 400; + + // SurfFeatureDetector detector( minHessian ); + // std::vector keypointsL, keypointsT, keypointsBotL, keypointsBotT, keypointsGML, keypointsGMT; + + // detector.detect( halfImage, keypointsL ); + // detector.detect( imageR, keypointsT ); + + // Mat imgKeypointsL; Mat imgKeypointsT; + + // //-- Detect keypoints that are within the box -LEFT + + // for (int i = 0; i < keypointsL.size(); i++) + // { + // if (keypointsL[i].pt.x < lBestLR.x && keypointsL[i].pt.x > lBestUL.x && + // keypointsL[i].pt.y < lBestLR.y && keypointsL[i].pt.y > lBestUL.y) + // { + // keypointsBotL.push_back(keypointsL[i]); + // } + // } + //-- Detect keypoints that are within the box - RIGHT + + // for (int i = 0; i < keypointsT.size(); i++) + // { + // if (keypointsT[i].pt.x < rBestLR.x && keypointsT[i].pt.x > rBestUL.x && + // keypointsT[i].pt.y < rBestLR.y && keypointsT[i].pt.y > rBestUL.y) + // { + // keypointsBotT.push_back(keypointsT[i]); + // } + // } + + // //-- Match points from within the box to the next frame +// std::cout << keypointsT.size() << std::endl; +// matchPoints(&keypointsBotL,&keypointsT,halfImage,tempIm,&keypointsGML,&keypointsGMT,5.1); + +// drawKeypoints( halfImage, keypointsGML, imgKeypointsL, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); +// imshow("with Keypoints",imgKeypointsL); +// printf("Matched %d points\n",keypointsGML.size()); + // if(keypointsGML.size()>lMostPoints) + // { + // lMostPoints = keypointsGML.size(); +// printf("---Highest Match is now %d\n",lMostPoints); +// printf("---Highest Match Rotation is %f\n",deg); +// } + + + +void Bot::getTemplateMatch(Mat image, Point* matchUL, Point* matchLR, Mat* templateImage,double* strongestResponse) +{ + //-- This function will take a template image, resize it to the box size, and find it in the image + //-- This is used as a check on the SURF features update of the box and the edge resize of the box + + //-- If the template image doesn't exist, make one + if(!templateImage->rows) + { + checkBounds(image,&upperLeft,&lowerRight); + *templateImage = image(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + } + + Mat result; + int resultCols = image.cols - templateImage->cols + 1; + int resultRows = image.rows - templateImage->rows + 1; + double minVal; double maxVal; Point minLoc; Point maxLoc; + + result.create( resultCols, resultRows, CV_32FC1 ); + + //-- Resize the template for the current box size + //Mat resizedTemplate; + //resize(*templateImage,resizedTemplate,Size( lowerRight.x - upperLeft.x,lowerRight.y - upperLeft.y)); + //-- Match and normalize + //CV_TIM_SQDIFF + matchTemplate(image, *templateImage, result, CV_TM_CCOEFF); + + //normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() ); + + //-- Grab the best match from the result matrix + minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() ); + //printf("Strongest Match :%f \n",maxVal); + *matchUL = maxLoc; + + //-- Set the other match coordinates + matchLR->x = matchUL->x + templateImage->cols; + matchLR->y = matchUL->y + templateImage->rows; + //-- Normalize response to the area of the template + *strongestResponse = maxVal/(templateImage->rows*templateImage->cols); +} + +void Bot::updateBoxSize(Mat image) +{ + //-- This function uses a sparse set of edges to resize the box describing the car + //-- This assumes there are many edges on the car, and space between the car and edges in the world + //-- This will drop small edges on the boundaries, to account for edges like the road that are always near the car + + //-- First determine the size of the bot + + //-- Resize the box + + //-- Match the closest template to get a rotation + + + //-- Create an image using Canny edge detection + Mat subImage,edges,subEdge; + + blur(image, edges, Size(5,5) ); + + Canny(edges,edges,79,102,3); + subEdge = edges(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + + //-- Get the current sum of gradients to compare against + float currentGradient = sum(subEdge)[0]; + + //-- This gives us a history of gradient sums to use to check if the current block is similar to the last block + //-- This did not provide enough information to spend time implementing + gradientHistory.push_back((int)currentGradient/255); + double gAvg = std::accumulate(gradientHistory.begin(), gradientHistory.end(), 0) / (double)gradientHistory.size(); + + //-- Expand the box and see if the increase in captured edges is large enough + /* + for (int i=0;i<4;i++) + { + Point newUL(upperLeft.x-2*i,upperLeft.y-2*i); + Point newLR(lowerRight.x+2*i,lowerRight.y+2*i); + checkBounds(edges,&newUL,&newLR); + subEdge = edges(Range(newUL.y,newLR.y),Range(newUL.x,newLR.x)); + //-- If there are enough extra edges, keep the new box size + if (sum(subEdge)[0]/255 > currentGradient/255 + 25) + { + upperLeft = newUL; + lowerRight = newLR; + } + } + + //-- The box has been greedily expanded, now contract it until there is more than noise on the edges + int noiseThreshold = 8; + while(sum(edges(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x-1,upperLeft.x+1)))[0]/255 < noiseThreshold && + upperLeft.x < lowerRight.x - 50) + { + upperLeft.x += 1; + } + + while(sum(edges(Range(upperLeft.y-1,upperLeft.y+1),Range(upperLeft.x,lowerRight.x)))[0]/255 < noiseThreshold && + upperLeft.y < lowerRight.y - 30) + { + upperLeft.y += 1; + } + + while(sum(edges(Range(upperLeft.y,lowerRight.y),Range(lowerRight.x-1,lowerRight.x+1)))[0]/255 < noiseThreshold && + upperLeft.x < lowerRight.x - 50) + { + lowerRight.x -= 1; + } + + while(sum(edges(Range(lowerRight.y-1,lowerRight.y+1),Range(upperLeft.x,lowerRight.x)))[0]/255 < noiseThreshold && + upperLeft.y < lowerRight.y - 30) + { + lowerRight.y -= 1; + } + + */ + checkBounds(edges,&upperLeft,&lowerRight); + rectangle(edges,upperLeft,lowerRight,150); + + Point matchUL, matchLR; + double sResponse; + + //-- Use the current template image to search for the car in the image + getTemplateMatch(image,&matchUL,&matchLR,&templateIm, &sResponse); + + checkBounds(image, &matchUL, &matchLR); + + rectangle( edges, matchUL, matchLR, 100, 2, 8, 0 ); + + //-- Allow for error in template matching, up to the overlap factor + double xOverlap,yOverlap,overlapFactor; + //-- This will consider success anything more than 50% overlap + overlapFactor = 0.5; + xOverlap = (lowerRight.x - upperLeft.x)*overlapFactor; + yOverlap = (lowerRight.y - upperLeft.y)*overlapFactor; + + if((matchUL.x < upperLeft.x + xOverlap && + matchUL.x > upperLeft.x - xOverlap && + matchUL.y < upperLeft.y + yOverlap && + matchUL.y > upperLeft.y - yOverlap) ) + { + upperLeft = matchUL; + lowerRight = matchLR; + //-- The template matches the current box and it has been 20 frames since we updated the template + if(updateTemplate==50) + { + templateIm = image(Range(matchUL.y,matchLR.y),Range(matchUL.x,matchLR.x)); updateTemplate = 0; + printf("------------------- Updated template --------------------\n"); + } + else updateTemplate++; + //-- We are tracking the car + inertialTracking = false; + } + else + { + //-- The template isn't matching the box + inertialTracking = true; + } + + //imshow("Canny Edge Detector",edges); + waitKey(10); +} + +void Bot::checkBounds(Mat image, Point* uL, Point* lR) +{ + //If any point doesn't exist, set it to 0 + if(!uL->x) uL->x = 0; + if(!uL->y) uL->y = 0; + if(!lR->x) lR->x = 0; + if(!lR->y) lR->y = 0; + + //Fix the box points if they are outside of bounds + //The smallest box is 50,30 + if (uL->x < 1) uL->x = 1; + if (uL->x > image.size().width) uL->x = image.size().width - 52; + if (lR->x < uL->x + 50) lR->x = uL->x + 50; + if (lR->x > image.size().width) lR->x = image.size().width - 1; + + + if (uL->y < 1) uL->y = 1; + if (uL->y > image.size().height) uL->y = image.size().height - 32; + if (lR->y < uL->y + 30) lR->y = uL->y + 30; + if (lR->y > image.size().height) lR->y = image.size().height - 1; + +} + diff --git a/src/bot.cpp~ b/src/bot.cpp~ new file mode 100644 index 0000000..08391c1 --- /dev/null +++ b/src/bot.cpp~ @@ -0,0 +1,1187 @@ +//This is the Car class +//The main focus is on two public functions to update the position and size of the tracking box +//The updateBoxSize function can be modified to be more robust for other image sets +// +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#include "perception_system/bot.h" +using namespace cv; + +Bot::Bot() +{ + upperLeft.x = 1; + upperLeft.y = 1; + lowerRight.x = 100; + lowerRight.y = 100; + showImages = false; + //checkBounds(initialIm, &upperLeft, &lowerRight); + //templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + updateTemplate = 1; +} + + +Bot::Bot(int sI) +{ + if(sI == 1) + showImages = true; + else + showImages = false; + upperLeft.x = 1; + upperLeft.y = 1; + lowerRight.x = 100; + lowerRight.y = 100; + inertialTracking = false; + //checkBounds(initialIm, &upperLeft, &lowerRight); + //templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + updateTemplate = 1; +} + +Bot::Bot(Point uL, Point lR) +{ + upperLeft = uL; + lowerRight = lR; + inertialTracking = false; + //checkBounds(initialIm, &upperLeft, &lowerRight); + //templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + updateTemplate = 1; +} + +Bot::~Bot() +{ +} + +void Bot::matchPoints(std::vector* keypointsIn1, std::vector* keypointsIn2,Mat image1, Mat image2, std::vector* keypointsOut1, std::vector* keypointsOut2, int thresh) +{ + if(!keypointsIn1->size() || !keypointsIn2->size()) + { + //-- Detect the keypoints using SURF Detector + int minHessian = 400; + + SurfFeatureDetector detector( minHessian ); + + detector.detect( image1, *keypointsIn1 ); + detector.detect( image2, *keypointsIn2 ); + } + + //-- Extract features for matching + SurfDescriptorExtractor extractor; + + Mat descriptors1, descriptors2; + + extractor.compute(image1, *keypointsIn1, descriptors1); + extractor.compute(image2, *keypointsIn2, descriptors2); + + FlannBasedMatcher matcher; + std::vector matches; + + //-- Match the car SURF points with all the next images SURF points + matcher.match(descriptors1,descriptors2, matches); + + //-- Find match strengths + double maxDist = 0; double minDist = 100; + + for( int i = 0; i < descriptors1.rows; i++ ) + { double dist = matches[i].distance; + if( dist < minDist ) minDist = dist; + if( dist > maxDist ) maxDist = dist; + } + + //-- Keep good matches + std::vector< DMatch > goodMatches; + + for( int i = 0; i < descriptors1.rows; i++ ) + { if( matches[i].distance <= max(thresh*minDist, 0.02) ) + { goodMatches.push_back( matches[i]); } + } + + //-- Fill the good matches keypoint vectors + for ( int i = 0; i < goodMatches.size(); i++) + { + (*keypointsOut1).push_back((*keypointsIn1)[goodMatches[i].queryIdx]); + (*keypointsOut2).push_back((*keypointsIn2)[goodMatches[i].trainIdx]); + } + +} + +void Bot::findGoalPos(Mat imageL, Mat imageR, float *xGoal, float *yGoal, float *tGoal) +{ + double lBestResponse=100; + int lMostPoints=0; + int lRotI; + float lBestDeg, lScale; + Point lBestUL,lBestLR; + + double rBestResponse=100; + int rRotI; + float rBestDeg, rScale; + Point rBestUL,rBestLR; + + //-- Check left image for goal + + for (float scale=.7;scale < 2.00; scale=scale+.10) + { + Point locUL,locLR; + double sResponse; + float deg; + char imName[30]; + Mat tempIm,tempEdges,lTempEdges; + Size scaleSize; + sprintf(imName,"images/templates/20APR/leftGoal.jpg"); + //printf("%s %f \n",imName,scale); + tempIm = imread(imName, CV_LOAD_IMAGE_COLOR); + scaleSize.height = tempIm.rows*scale; + scaleSize.width = tempIm.cols*scale; + resize(tempIm,tempIm,scaleSize); + + //Do it on Edges + //blur(tempIm,tempEdges,Size(3,3)); + //Canny(tempEdges,tempEdges,120,140,3); + + //Mat halfImage = lTempEdges(Range(imageL.cols/2,imageL.cols),Range(1,imageL.rows)); + Mat halfImage = imageL(Range(imageL.rows/2,imageL.rows),Range(1,imageL.cols)); + + //blur(halfImage,lTempEdges,Size(3,3)); + //Canny(lTempEdges,lTempEdges,120,140,3); + + //templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + if(showImages) + { + imshow("goalTemplate",tempIm); + moveWindow("goalTemplate",10,500); + //imshow("edges",lTempEdges); + waitKey(10); + } + + getTemplateMatch(imageL,&locUL,&locLR, &tempIm, &sResponse); + deg = 0; + + Mat imageS = imageL.clone(); + rectangle(imageS,locUL,locLR,10,3); + //sResponse = sResponse/(8900*pow(scale,2)); + if (lBestResponse < sResponse) + { + lScale = scale; + lRotI = 1; + lBestUL = locUL; + lBestLR = locLR; + lBestResponse = sResponse; + lBestDeg = deg; + printf("lBest Response: %f\n",lBestResponse); + } + if(showImages) + { + rectangle(imageS,lBestUL,lBestLR,205,3); + imshow("Left Image Goal",imageS); + moveWindow("Left Image Goal",200,400); + waitKey(10); + } + } + + + //-- Check right image for goal + for (float scale=.7;scale < 2.00; scale=scale+.10) + { + Point locUL,locLR; + double sResponse; + float deg; + char imName[30]; + Mat tempIm,tempEdges,rTempEdges; + Size scaleSize; + sprintf(imName,"images/templates/20APR/rightGoal.jpg"); + //printf("%s %f \n",imName,scale); + tempIm = imread(imName, CV_LOAD_IMAGE_COLOR); + scaleSize.height = tempIm.rows*scale; + scaleSize.width = tempIm.cols*scale; + + resize(tempIm,tempIm,scaleSize); + + //Do it on Edges + //blur(tempIm,tempEdges,Size(3,3)); + //Canny(tempEdges,tempEdges,100,120,3); + + //blur(imageR,rTempEdges,Size(3,3)); + //Canny(rTempEdges,rTempEdges,100,120,3); + + Mat halfImage = imageR(Range(imageR.rows/2,imageR.rows),Range(1,imageR.cols)); + if(showImages) + { + imshow("goalTemplate",tempIm); + //waitKey(10); + } + getTemplateMatch(imageR,&locUL,&locLR, &tempIm, &sResponse); + deg = 0; + //printf("LEFT\ndegree: %f \ntemplate: %d \nlocUL-x: %d \nlocUL-y: %d \nlocLR-x: %d\nlocLR-y: %d\n",deg,i,locUL.x,locUL.y,locLR.x,locLR.y); + Mat imageS = imageR.clone(); + rectangle(imageS,locUL,locLR,10,3); + //sResponse = sResponse/(8900*pow(scale,2)); + if (rBestResponse < sResponse) + { + rScale = scale; + rRotI = 1; + rBestUL = locUL; + rBestLR = locLR; + rBestResponse = sResponse; + rBestDeg = deg; + printf("rBest Response: %f\n",rBestResponse); + } + if(showImages) + { + rectangle(imageS,rBestUL,rBestLR,200,3); + imshow("Right Image Goal",imageS); + moveWindow("Right Image Goal",200,500); + } + //rectangle(imgKeypoints1,upperLeft,lowerRight,10,3); + waitKey(10); + } + + + //-- Estimate a box + + //-- Match points inside the boxes in the Left and Right images + int minHessian = 400; + + SurfFeatureDetector detector( minHessian ); + + std::vector keypointsL, keypointsR, keypointsBotL, keypointsBotR, keypointsGML, keypointsGMR; + + detector.detect( imageL, keypointsL ); + detector.detect( imageR, keypointsR ); + + Mat imgKeypointsL; Mat imgKeypointsR; + + // //-- Detect keypoints that are within the box -LEFT + + Mat halfImage = imageR(Range(imageR.rows/2,imageR.rows),Range(1,imageR.cols)); + + + for (int i = 0; i < keypointsL.size(); i++) + { + if (keypointsL[i].pt.x < lBestLR.x && keypointsL[i].pt.x > lBestUL.x && + keypointsL[i].pt.y < (lBestLR.y /*+ imageL.rows/2 */) && keypointsL[i].pt.y > (lBestUL.y /*+ imageL.rows/2*/)) + { + keypointsBotL.push_back(keypointsL[i]); + } + } + // //-- Detect keypoints that are within the box - RIGHT + + for (int i = 0; i < keypointsR.size(); i++) + { + if (keypointsR[i].pt.x < rBestLR.x && keypointsR[i].pt.x > rBestUL.x && + keypointsR[i].pt.y < (rBestLR.y /*+ imageR.rows/2*/) && keypointsR[i].pt.y > (rBestUL.y /*+ imageR.rows/2*/)) + { + keypointsBotR.push_back(keypointsR[i]); + } + } + // //-- Match points from within the box to the next frame + matchPoints(&keypointsBotL,&keypointsBotR,imageL,imageR,&keypointsGML,&keypointsGMR,5.1); + + + //-- Triangulate distance + + Mat lProj,rProj; + string paramsyml = "projmats.yml"; + FileStorage fsP(paramsyml,FileStorage::READ); + fsP["P1"] >>lProj; + fsP["P2"] >>rProj; + + Mat worldPoints,imageWithKeyPointsL,imageWithKeyPointsR; + std::vector lPoints,rPoints; + for (int i = 0; i < keypointsGML.size(); i++) + { + lPoints.push_back(keypointsGML[i].pt); + } + for (int i = 0; i < keypointsGMR.size(); i++) + { + rPoints.push_back(keypointsGMR[i].pt); + } + drawKeypoints( imageL, keypointsGML, imageWithKeyPointsL, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); + drawKeypoints( imageR, keypointsGMR, imageWithKeyPointsR, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); + + imshow("With Keypoints L - GOAL", imageWithKeyPointsL); + imshow("With Keypoints R - GOAL", imageWithKeyPointsR); + waitKey(10); + + Point2f tempPoint; + // //left 0,0 + tempPoint.x = 206; + tempPoint.y = 568; + lPoints.push_back(tempPoint); + // //left 0,10 + tempPoint.x = 252; + tempPoint.y = 409; + lPoints.push_back(tempPoint); + //left 10,0 + tempPoint.x = 1134; + tempPoint.y = 502; + lPoints.push_back(tempPoint); + //left 10,10 + tempPoint.x = 762; + tempPoint.y = 395; + lPoints.push_back(tempPoint); + + //right 0,0 + tempPoint.x = 71; + tempPoint.y = 636; + rPoints.push_back(tempPoint); + //right 0,10 + tempPoint.x = 169; + tempPoint.y = 474; + rPoints.push_back(tempPoint); + //right 10,0 + tempPoint.x = 1018; + tempPoint.y = 545; + rPoints.push_back(tempPoint); + // //right 10,10 + tempPoint.x = 689; + tempPoint.y = 449; + rPoints.push_back(tempPoint); + + //std::cout << "\n2D POINTS" << std::endl << lPoints << std::endl << rPoints < x,y,z; + for (int i = 0; i < (worldPoints.cols - 4); i++) + { + worldPoints.at(0,i) /= worldPoints.at(3,i); + x.push_back(worldPoints.at(0,i)); + worldPoints.at(1,i) /= worldPoints.at(3,i); + y.push_back(worldPoints.at(1,i)); + worldPoints.at(2,i) /= worldPoints.at(3,i); + z.push_back(worldPoints.at(2,i)); + worldPoints.at(3,i) /= worldPoints.at(3,i); + } + + float avgX = std::accumulate(x.begin(), x.end(), 0) / (float)x.size(); + float avgY = std::accumulate(y.begin(), y.end(), 0) / (float)y.size(); + float avgZ = std::accumulate(z.begin(), z.end(), 0) / (float)z.size(); + + //std::cout<< "\nworldPoints: "<< std::endl << worldPoints << std::endl; + //printf("WorldPoints: \n",worldPoints); + printf("Avg X: %f\n",avgX); + printf("Avg Y: %f\n",avgY); + printf("Avg Z: %f\n",avgZ); + + for (int i = (worldPoints.cols - 4); i < worldPoints.cols; i++) + { + //printf("i: %d\n",i); + worldPoints.at(0,i) /= worldPoints.at(3,i); + //printf("X: %f\n",worldPoints.at(0,i)); + worldPoints.at(1,i) /= worldPoints.at(3,i); + //printf("Y: %f\n",worldPoints.at(1,i)); + worldPoints.at(2,i) /= worldPoints.at(3,i); + //printf("Z: %f\n",worldPoints.at(2,i)); + worldPoints.at(3,i) /= worldPoints.at(3,i); + } + + Mat boundW = worldPoints(Range(0,(worldPoints.rows-1)),Range(worldPoints.cols-4,worldPoints.cols)); + + std::cout << std::endl << boundW << std::endl; + + //-- Map it to something based on the boundary values + if(lBestDeg == 0 && rBestDeg > 180) + { + lBestDeg = 360.0; + } + if(rBestDeg == 0 && lBestDeg > 180) + { + rBestDeg = 360.0; + } + + // printf("\nScale: %f\n",(lScale + rScale)/2); + + //printf("leftLoc %d %d\n",(lBestUL.x + lBestLR.x)/2,(lBestUL.y + lBestLR.y)/2); + //printf("rightLoc %d %d\n",(rBestUL.x + rBestLR.x)/2,(rBestUL.y + rBestLR.y)/2); + + // Mat boundP = Mat::zeros(3, 4, CV_32F); + + // boundP.at(1,1) = 10.0; + // boundP.at(0,2) = 10.0; + // boundP.at(0,3) = 10.0; + // boundP.at(1,3) = 10.0; + + // //std::cout << boundP << std::endl; + + // Mat warp_mat( 3, 4, CV_32FC1 ); + //warp_mat = getAffineTransform( boundW,boundP); + //std::vector::const_iterator first = worldPoints.end() - 4; + // std::vector::const_iterator last = worldPoints.end(); + // std::vector inBound(first, last); + + if(lBestDeg == 0 && rBestDeg > 180) + { + lBestDeg = 360.0; + } + if(rBestDeg == 0 && lBestDeg > 180) + { + rBestDeg = 360.0; + } + + float bestDeg = (lBestDeg + rBestDeg)/2 + 45.0; + if (bestDeg > 360) + { + bestDeg = bestDeg - 360.0; + } + printf("\nScale: %f\n",(lScale + rScale)/2); + + printf("leftLoc %d %d\n",(lBestUL.x + lBestLR.x)/2,(lBestUL.y + lBestLR.y)/2); + printf("rightLoc %d %d\n",(rBestUL.x + rBestLR.x)/2,(rBestUL.y + rBestLR.y)/2); + + + printf("\nBest Estimate of Rotation: %f\n",bestDeg); + + //-- Translate scale and position + float yTrans = 5.8365 - 3.3654*(lScale + rScale)/2; + float xLeftTrans = ((lBestUL.x + lBestLR.x)/2 - 250)*(4.25/(1100-250)); + float xRightTrans = ((rBestUL.x + rBestLR.x)/2 - 150)*(4.25/(1000-150)); + + *yGoal = yTrans; + *xGoal = (xLeftTrans+xRightTrans)/2; + *tGoal = bestDeg; + + printf("Estimate of X,Y pos of GOAL: %f %f\n",(xLeftTrans+xRightTrans)/2,yTrans); + + + waitKey(1); + +} + +void Bot::updateBoxPos(Mat imageL, Mat imageR, float *xBot, float *yBot, float *tBot) +{ + double lBestResponse=100; + int lMostPoints=0; + int lRotI; + float lBestDeg, lScale; + Point lBestUL,lBestLR; + + double rBestResponse=100; + int rRotI; + float rBestDeg, rScale; + Point rBestUL,rBestLR; + + //-- Check left image for robot + for (int i=1;i<9;i++) + { + for (float scale=.6;scale < 2.00; scale=scale+.15) + { + Point locUL,locLR; + double sResponse; + float deg; + char imName[30]; + Mat tempIm,tempEdges,lTempEdges; + Size scaleSize; + sprintf(imName,"images/templates/20APR/left%03d.jpg",i); + //printf("%s %f \n",imName,scale); + tempIm = imread(imName, CV_LOAD_IMAGE_COLOR); + scaleSize.height = tempIm.rows*scale; + scaleSize.width = tempIm.cols*scale; + + //printf("scale height %d width %d\n",scaleSize.height,scaleSize.width); + sprintf(imName,"%f d",(i*360.0/8)); + + resize(tempIm,tempIm,scaleSize); + + //Do it on Edges + //blur(tempIm,tempEdges,Size(3,3)); + //Canny(tempEdges,tempEdges,120,140,3); + + //Mat halfImage = lTempEdges(Range(imageL.cols/2,imageL.cols),Range(1,imageL.rows)); + Mat halfImage = imageL(Range(imageL.rows/2,imageL.rows),Range(1,imageL.cols)); + + //blur(halfImage,lTempEdges,Size(3,3)); + //Canny(lTempEdges,lTempEdges,120,140,3); + + //templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + if(showImages) + { + imshow("template",tempIm); + moveWindow("template",10,10); + //imshow("edges",lTempEdges); + waitKey(1); + } + + getTemplateMatch(halfImage,&locUL,&locLR, &tempIm, &sResponse); + // getTemplateMatch(lTempEdges,&locUL,&locLR, &tempEdges, &sResponse); + // getTemplateMatch(imageL,&locUL,&locLR, &tempIm, &sResponse); + //locUL.y = locUL.y + imageL.rows/2; + //locLR.y = locLR.y + imageL.rows/2; + deg = (i-1)*(360.0/8); + //printf("LEFT\ndegree: %f \ntemplate: %d \nlocUL-x: %d \nlocUL-y: %d \nlocLR-x: %d\nlocLR-y: %d\n",deg,i,locUL.x,locUL.y,locLR.x,locLR.y); + + Mat imageS = halfImage.clone(); + rectangle(imageS,locUL,locLR,10,3); + //sResponse = sResponse/(8900*pow(scale,2)); + if (lBestResponse < sResponse) + { + lScale = scale; + lRotI = i; + lBestUL = locUL; + lBestLR = locLR; + lBestResponse = sResponse; + lBestDeg = deg; + //printf("lBest Response: %f\n",lBestResponse); + } + if(showImages) + { + rectangle(imageS,lBestUL,lBestLR,205,3); + imshow("Left Image",imageS); + moveWindow("Left Image",300,10); + } + //rectangle(imgKeypoints1,upperLeft,lowerRight,10,3); + + //-- MATCHING SURF FEATURES + + //-- Match points inside the boxes in the Left and template images + int minHessian = 400; + + SurfFeatureDetector detector( minHessian ); + std::vector keypointsL, keypointsT, keypointsBotL, keypointsBotT, keypointsGML, keypointsGMT; + + detector.detect( halfImage, keypointsL ); + detector.detect( tempIm, keypointsT ); + + Mat imgKeypointsL; Mat imgKeypointsT; + + //-- Detect keypoints that are within the box -LEFT + + for (int i = 0; i < keypointsL.size(); i++) + { + if (keypointsL[i].pt.x < lBestLR.x && keypointsL[i].pt.x > lBestUL.x && + keypointsL[i].pt.y < lBestLR.y && keypointsL[i].pt.y > lBestUL.y) + { + keypointsBotL.push_back(keypointsL[i]); + } + } + //-- Detect keypoints that are within the box - RIGHT + + // for (int i = 0; i < keypointsT.size(); i++) + // { + // if (keypointsT[i].pt.x < rBestLR.x && keypointsT[i].pt.x > rBestUL.x && + // keypointsT[i].pt.y < rBestLR.y && keypointsT[i].pt.y > rBestUL.y) + // { + // keypointsBotT.push_back(keypointsT[i]); + // } + // } + + // //-- Match points from within the box to the next frame + std::cout << keypointsT.size() << std::endl; + matchPoints(&keypointsBotL,&keypointsT,halfImage,tempIm,&keypointsGML,&keypointsGMT,3.1); + + drawKeypoints( halfImage, keypointsGML, imgKeypointsL, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); + imshow("with Keypoints",imgKeypointsL); + printf("Matched %d points\n",keypointsGML.size()); + if(keypointsGML.size()>lMostPoints) + { + lMostPoints = keypointsGML.size(); + printf("---Highest Match is now %d\n",lMostPoints); + printf("---Highest Match Rotation is %f\n",deg); + } + + waitKey(1); + } + } + + printf("\nLBest Estimate of Left Rotation: %f\n",lBestDeg); + + //-- Check right image for robot + for (int i=1;i<9;i++) + { + for (float scale=.6;scale < 2.00; scale=scale+.15) + { + Point locUL,locLR; + double sResponse; + float deg; + char imName[30]; + Mat tempIm,tempEdges,rTempEdges; + Size scaleSize; + sprintf(imName,"images/templates/20APR/right%03d.jpg",i); + //printf("%s %f \n",imName,scale); + tempIm = imread(imName, CV_LOAD_IMAGE_COLOR); + scaleSize.height = tempIm.rows*scale; + scaleSize.width = tempIm.cols*scale; + + // printf("scale height %d width %d\n",scaleSize.height,scaleSize.width); + sprintf(imName,"%f d",(i*360.0/8)); + + resize(tempIm,tempIm,scaleSize); + + //Do it on Edges + //blur(tempIm,tempEdges,Size(3,3)); + //Canny(tempEdges,tempEdges,100,120,3); + + + //blur(imageR,rTempEdges,Size(3,3)); + //Canny(rTempEdges,rTempEdges,100,120,3); + + Mat halfImage = imageR(Range(imageR.rows/2,imageR.rows),Range(1,imageR.cols)); + if(showImages) + { + imshow("template",tempIm); + moveWindow("template",10,10); + waitKey(1); + } + getTemplateMatch(halfImage,&locUL,&locLR, &tempIm, &sResponse); + deg = (i-1)*(360.0/8); + //printf("LEFT\ndegree: %f \ntemplate: %d \nlocUL-x: %d \nlocUL-y: %d \nlocLR-x: %d\nlocLR-y: %d\n",deg,i,locUL.x,locUL.y,locLR.x,locLR.y); + Mat imageS = halfImage.clone(); + rectangle(imageS,locUL,locLR,10,3); + //sResponse = sResponse/(8900*pow(scale,2)); + if (rBestResponse < sResponse) + { + rScale = scale; + rRotI = i; + rBestUL = locUL; + rBestLR = locLR; + rBestResponse = sResponse; + rBestDeg = deg; + //printf("rBest Response: %f\n",rBestResponse); + } + if(showImages) + { + rectangle(imageS,rBestUL,rBestLR,200,3); + imshow("Right Image",imageS); + moveWindow("Right Image",300,250); + + waitKey(1); + } + } + } + + printf("\nBest Estimate of Right Rotation: %f\n",rBestDeg); + + //-- Estimate a box + + //-- Match points inside the boxes in the Left and Right images + int minHessian = 400; + SurfFeatureDetector detector( minHessian ); + + std::vector keypointsL, keypointsR, keypointsBotL, keypointsBotR, keypointsGML, keypointsGMR; + + detector.detect( imageL, keypointsL ); + detector.detect( imageR, keypointsR ); + Mat imgKeypointsL; Mat imgKeypointsR; + + //-- Detect keypoints that are within the box -LEFT + + Mat halfImage = imageR(Range(imageR.rows/2,imageR.rows),Range(1,imageR.cols)); + + + for (int i = 0; i < keypointsL.size(); i++) + { + if (keypointsL[i].pt.x < lBestLR.x && keypointsL[i].pt.x > lBestUL.x && + keypointsL[i].pt.y < (lBestLR.y + imageL.rows/2) && keypointsL[i].pt.y > (lBestUL.y + imageL.rows/2)) + { + keypointsBotL.push_back(keypointsL[i]); + } + } + //-- Detect keypoints that are within the box - RIGHT + + for (int i = 0; i < keypointsR.size(); i++) + { + if (keypointsR[i].pt.x < rBestLR.x && keypointsR[i].pt.x > rBestUL.x && + keypointsR[i].pt.y < (rBestLR.y + imageR.rows/2) && keypointsR[i].pt.y > (rBestUL.y + imageR.rows/2)) + { + keypointsBotR.push_back(keypointsR[i]); + } + } + //-- Match points from within the box to the next frame + matchPoints(&keypointsBotL,&keypointsBotR,imageL,imageR,&keypointsGML,&keypointsGMR,5.1); + + + //-- Triangulate distance + + Mat lProj,rProj; + string paramsyml = "projmats.yml"; + FileStorage fsP(paramsyml,FileStorage::READ); + fsP["P1"] >>lProj; + fsP["P2"] >>rProj; + + // std::cout<<"Printing contents of proj mats :"< stLP,stRP; + // string paramsyml = "standardPoints.yml"; + // FileStorage fsP(paramsyml,FileStorage::READ); + // fsP["leftP"] >>stLP; + // fsP["rightP"] >>stRP; + + Mat worldPoints,imageWithKeyPointsL,imageWithKeyPointsR; + std::vector lPoints,rPoints; + for (int i = 0; i < keypointsGML.size(); i++) + { + lPoints.push_back(keypointsGML[i].pt); + } + for (int i = 0; i < keypointsGMR.size(); i++) + { + rPoints.push_back(keypointsGMR[i].pt); + } + drawKeypoints( imageL, keypointsGML, imageWithKeyPointsL, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); + drawKeypoints( imageR, keypointsGMR, imageWithKeyPointsR, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); + + imshow("With Keypoints L", imageWithKeyPointsL); + imshow("With Keypoints R", imageWithKeyPointsR); + waitKey(10); + + Point2f tempPoint; + // //left 0,0 + tempPoint.x = 206; + tempPoint.y = 568; + lPoints.push_back(tempPoint); + // //left 0,10 + tempPoint.x = 252; + tempPoint.y = 409; + lPoints.push_back(tempPoint); + // //left 10,0 + tempPoint.x = 1134; + tempPoint.y = 502; + lPoints.push_back(tempPoint); + // //left 10,10 + tempPoint.x = 762; + tempPoint.y = 395; + lPoints.push_back(tempPoint); + + // //right 0,0 + tempPoint.x = 71; + tempPoint.y = 636; + rPoints.push_back(tempPoint); + // //right 0,10 + tempPoint.x = 169; + tempPoint.y = 474; + rPoints.push_back(tempPoint); + //right 10,0 + tempPoint.x = 1018; + tempPoint.y = 545; + rPoints.push_back(tempPoint); + // //right 10,10 + tempPoint.x = 689; + tempPoint.y = 449; + rPoints.push_back(tempPoint); + + // //std::cout << "\n2D POINTS" << std::endl << lPoints << std::endl << rPoints < x,y,z; + for (int i = 0; i < (worldPoints.cols - 4); i++) + { + worldPoints.at(0,i) /= worldPoints.at(3,i); + x.push_back(worldPoints.at(0,i)); + worldPoints.at(1,i) /= worldPoints.at(3,i); + y.push_back(worldPoints.at(1,i)); + worldPoints.at(2,i) /= worldPoints.at(3,i); + z.push_back(worldPoints.at(2,i)); + worldPoints.at(3,i) /= worldPoints.at(3,i); + } + + float avgX = std::accumulate(x.begin(), x.end(), 0) / (float)x.size(); + float avgY = std::accumulate(y.begin(), y.end(), 0) / (float)y.size(); + float avgZ = std::accumulate(z.begin(), z.end(), 0) / (float)z.size(); + + //std::cout<< "\nworldPoints: "<< std::endl << worldPoints << std::endl; + // // printf("WorldPoints: \n",worldPoints); + printf("Avg X: %f\n",avgX); + printf("Avg Y: %f\n",avgY); + printf("Avg Z: %f\n",avgZ); + + for (int i = (worldPoints.cols - 4); i < worldPoints.cols; i++) + { + //printf("i: %d\n",i); + worldPoints.at(0,i) /= worldPoints.at(3,i); + // //printf("X: %f\n",worldPoints.at(0,i)); + worldPoints.at(1,i) /= worldPoints.at(3,i); + // //printf("Y: %f\n",worldPoints.at(1,i)); + worldPoints.at(2,i) /= worldPoints.at(3,i); + // // printf("Z: %f\n",worldPoints.at(2,i)); + worldPoints.at(3,i) /= worldPoints.at(3,i); + } + + Mat boundW = worldPoints(Range(0,(worldPoints.rows-1)),Range(worldPoints.cols-4,worldPoints.cols)); + + std::cout << std::endl << boundW << std::endl; + + //-- Map it to something based on the boundary values + if(lBestDeg == 0 && rBestDeg > 180) + { + lBestDeg = 360.0; + } + if(rBestDeg == 0 && lBestDeg > 180) + { + rBestDeg = 360.0; + } + + float bestDeg = (lBestDeg + rBestDeg)/2 + 45.0; + if (bestDeg > 360) + { + bestDeg = bestDeg - 360.0; + } + printf("\nScale: %f\n",(lScale + rScale)/2); + + printf("leftLoc %d %d\n",(lBestUL.x + lBestLR.x)/2,(lBestUL.y + lBestLR.y)/2); + printf("rightLoc %d %d\n",(rBestUL.x + rBestLR.x)/2,(rBestUL.y + rBestLR.y)/2); + + + printf("\nBest Estimate of Rotation: %f\n",bestDeg); + + //-- Translate scale and position + float yTrans = 5.8365 - 3.3654*(lScale + rScale)/2; + float xLeftTrans = ((lBestUL.x + lBestLR.x)/2 - 250)*(4.25/(1100-250)); + float xRightTrans = ((rBestUL.x + rBestLR.x)/2 - 150)*(4.25/(1000-150)); + + *yBot = yTrans; + *xBot = (xLeftTrans+xRightTrans)/2; + *tBot = bestDeg; + + + printf("Estimate of X,Y pos: %f %f",(xLeftTrans+xRightTrans)/2,yTrans); + + waitKey(10); +} + +void Bot::getTemplateMatch(Mat image, Point* matchUL, Point* matchLR, Mat* templateImage,double* strongestResponse) +{ + //-- This function will take a template image, resize it to the box size, and find it in the image + //-- This is used as a check on the SURF features update of the box and the edge resize of the box + + //-- If the template image doesn't exist, make one + if(!templateImage->rows) + { + checkBounds(image,&upperLeft,&lowerRight); + *templateImage = image(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + } + + Mat result; + int resultCols = image.cols - templateImage->cols + 1; + int resultRows = image.rows - templateImage->rows + 1; + double minVal; double maxVal; Point minLoc; Point maxLoc; + + result.create( resultCols, resultRows, CV_32FC1 ); + + //-- Resize the template for the current box size + //Mat resizedTemplate; + //resize(*templateImage,resizedTemplate,Size( lowerRight.x - upperLeft.x,lowerRight.y - upperLeft.y)); + //-- Match and normalize + //CV_TIM_SQDIFF + matchTemplate(image, *templateImage, result, CV_TM_CCOEFF); + + //normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() ); + + //-- Grab the best match from the result matrix + minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() ); + //printf("Strongest Match :%f \n",maxVal); + *matchUL = maxLoc; + + //-- Set the other match coordinates + matchLR->x = matchUL->x + templateImage->cols; + matchLR->y = matchUL->y + templateImage->rows; + //-- Normalize response to the area of the template + *strongestResponse = maxVal/(templateImage->rows*templateImage->cols); +} + +void Bot::updateBoxSize(Mat image) +{ + //-- This function uses a sparse set of edges to resize the box describing the car + //-- This assumes there are many edges on the car, and space between the car and edges in the world + //-- This will drop small edges on the boundaries, to account for edges like the road that are always near the car + + //-- First determine the size of the bot + + //-- Resize the box + + //-- Match the closest template to get a rotation + + + //-- Create an image using Canny edge detection + Mat subImage,edges,subEdge; + + blur(image, edges, Size(5,5) ); + + Canny(edges,edges,79,102,3); + subEdge = edges(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + + //-- Get the current sum of gradients to compare against + float currentGradient = sum(subEdge)[0]; + + //-- This gives us a history of gradient sums to use to check if the current block is similar to the last block + //-- This did not provide enough information to spend time implementing + gradientHistory.push_back((int)currentGradient/255); + double gAvg = std::accumulate(gradientHistory.begin(), gradientHistory.end(), 0) / (double)gradientHistory.size(); + + //-- Expand the box and see if the increase in captured edges is large enough + /* + for (int i=0;i<4;i++) + { + Point newUL(upperLeft.x-2*i,upperLeft.y-2*i); + Point newLR(lowerRight.x+2*i,lowerRight.y+2*i); + checkBounds(edges,&newUL,&newLR); + subEdge = edges(Range(newUL.y,newLR.y),Range(newUL.x,newLR.x)); + //-- If there are enough extra edges, keep the new box size + if (sum(subEdge)[0]/255 > currentGradient/255 + 25) + { + upperLeft = newUL; + lowerRight = newLR; + } + } + + //-- The box has been greedily expanded, now contract it until there is more than noise on the edges + int noiseThreshold = 8; + while(sum(edges(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x-1,upperLeft.x+1)))[0]/255 < noiseThreshold && + upperLeft.x < lowerRight.x - 50) + { + upperLeft.x += 1; + } + + while(sum(edges(Range(upperLeft.y-1,upperLeft.y+1),Range(upperLeft.x,lowerRight.x)))[0]/255 < noiseThreshold && + upperLeft.y < lowerRight.y - 30) + { + upperLeft.y += 1; + } + + while(sum(edges(Range(upperLeft.y,lowerRight.y),Range(lowerRight.x-1,lowerRight.x+1)))[0]/255 < noiseThreshold && + upperLeft.x < lowerRight.x - 50) + { + lowerRight.x -= 1; + } + + while(sum(edges(Range(lowerRight.y-1,lowerRight.y+1),Range(upperLeft.x,lowerRight.x)))[0]/255 < noiseThreshold && + upperLeft.y < lowerRight.y - 30) + { + lowerRight.y -= 1; + } + + */ + checkBounds(edges,&upperLeft,&lowerRight); + rectangle(edges,upperLeft,lowerRight,150); + + Point matchUL, matchLR; + double sResponse; + + //-- Use the current template image to search for the car in the image + getTemplateMatch(image,&matchUL,&matchLR,&templateIm, &sResponse); + + checkBounds(image, &matchUL, &matchLR); + + rectangle( edges, matchUL, matchLR, 100, 2, 8, 0 ); + + //-- Allow for error in template matching, up to the overlap factor + double xOverlap,yOverlap,overlapFactor; + //-- This will consider success anything more than 50% overlap + overlapFactor = 0.5; + xOverlap = (lowerRight.x - upperLeft.x)*overlapFactor; + yOverlap = (lowerRight.y - upperLeft.y)*overlapFactor; + + if((matchUL.x < upperLeft.x + xOverlap && + matchUL.x > upperLeft.x - xOverlap && + matchUL.y < upperLeft.y + yOverlap && + matchUL.y > upperLeft.y - yOverlap) ) + { + upperLeft = matchUL; + lowerRight = matchLR; + //-- The template matches the current box and it has been 20 frames since we updated the template + if(updateTemplate==50) + { + templateIm = image(Range(matchUL.y,matchLR.y),Range(matchUL.x,matchLR.x)); updateTemplate = 0; + printf("------------------- Updated template --------------------\n"); + } + else updateTemplate++; + //-- We are tracking the car + inertialTracking = false; + } + else + { + //-- The template isn't matching the box + inertialTracking = true; + } + + //imshow("Canny Edge Detector",edges); + waitKey(10); +} + +void Bot::checkBounds(Mat image, Point* uL, Point* lR) +{ + //If any point doesn't exist, set it to 0 + if(!uL->x) uL->x = 0; + if(!uL->y) uL->y = 0; + if(!lR->x) lR->x = 0; + if(!lR->y) lR->y = 0; + + //Fix the box points if they are outside of bounds + //The smallest box is 50,30 + if (uL->x < 1) uL->x = 1; + if (uL->x > image.size().width) uL->x = image.size().width - 52; + if (lR->x < uL->x + 50) lR->x = uL->x + 50; + if (lR->x > image.size().width) lR->x = image.size().width - 1; + + + if (uL->y < 1) uL->y = 1; + if (uL->y > image.size().height) uL->y = image.size().height - 32; + if (lR->y < uL->y + 30) lR->y = uL->y + 30; + if (lR->y > image.size().height) lR->y = image.size().height - 1; + +} + + +bool Bot::unitTest(Mat image) +{ + bool allTests = true; + //-- This function is a simple example of unit testing + //-- There are frameworks that do this better + + Point matchUL,matchLR; + + //-- First test checkBounds + //-- This function is a check used throughout the class + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Empty Points\n"); + + matchUL.x = 0; + matchUL.y = 0; + matchLR.x = 0; + matchLR.y = 0; + + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Zero Points\n"); + + matchUL.x = image.size().width*2; + matchUL.y = image.size().height*2; + matchLR.x = image.size().width*2; + matchLR.y = image.size().height*2; + + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Out of Bounds Points\n"); + + matchUL.x = image.size().width - 10; + matchUL.y = image.size().height - 10; + matchLR.x = 2; + matchLR.y = 2; + + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Reversed Points\n"); + + if(templateIm.rows) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" Constructor Template Doesn't Exist\n"); + + //-- Test getTemplateMatch + Mat emptyTemplate; + double sResponse; + getTemplateMatch(image,&matchUL,&matchLR,&emptyTemplate, &sResponse); + if(matchUL== upperLeft) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" Empty Template Doesn't Match\n"); + + getTemplateMatch(image, &matchUL, &matchLR, &templateIm, &sResponse); + if(matchUL== upperLeft) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" Constructor Template Doesn't Match\n"); + + //-- Test matchPoints + std::vector i1,i2,o1,o2; + matchPoints(&i1,&i2,image,image,&o1,&o2,5); + if(o1.size()) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" matchPoints Empty Keypoints\n"); + + if (o1[0].pt.x == o2[0].pt.x && o1[0].pt.y == o2[0].pt.y) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" matchPoints Detector Identical Image\n"); + //-- Test updateBoxPos + matchUL = upperLeft; + matchLR = lowerRight; + //updateBoxPos(image,image,float*, float*, float*); + if(matchUL.x == upperLeft.x) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" updateBoxPos Identical Image 0 Movement\n"); + + //-- Test updateBoxSize - check that the box is about the same size + matchUL = upperLeft; + matchLR = lowerRight; + updateBoxSize(image); + if(matchUL.x < upperLeft.x + 5 && + matchUL.x > upperLeft.x - 5 && + matchUL.y < upperLeft.y + 5 && + matchUL.y > upperLeft.y - 5 && + matchLR.x < lowerRight.x + 5 && + matchLR.x > lowerRight.x - 5 && + matchLR.x < lowerRight.x + 5 && + matchLR.x > lowerRight.x - 5) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" updateBoxSize Identical Image\n"); + + return allTests; +} diff --git a/src/bot.h~ b/src/bot.h~ new file mode 100644 index 0000000..50c6d68 --- /dev/null +++ b/src/bot.h~ @@ -0,0 +1,54 @@ +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#ifndef CAR_H +#define CAR_H + + +#include +#include +#include +#include +#include +#include +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/core/core.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/nonfree/nonfree.hpp" + +using namespace cv; + +class Car +{ + public: + Car(Point uL, Point lR, Mat initialIm); + ~Car(); + + void updateBoxPos(Mat image1, Mat image2); + void updateBoxSize(Mat image); + bool unitTest(Mat image); + + private: + Point upperLeft; + Point lowerRight; + bool inertialTracking; + Mat templateIm; + int updateTemplate; + + std::vector confirmedFeatures; + std::vector gradientHistory; + std::vector xHistory; + std::vector yHistory; + + void checkBounds(Mat image, Point* uL, Point* lR); + void getTemplateMatch(Mat image, Point* matchUL, Point* matchLR, Mat* templateImage); + void matchPoints(std::vector* keypointsIn1, std::vector* keypointsIn2,Mat image1, Mat image2, std::vector* keypointsOut1, std::vector* keypointsOut2, int thresh); + + +}; + + +#endif diff --git a/src/bot_tracker.cpp b/src/bot_tracker.cpp new file mode 100644 index 0000000..bb32afa --- /dev/null +++ b/src/bot_tracker.cpp @@ -0,0 +1,55 @@ +//This file uses the Car class to track a vehicle in a set of images +//Change this file to track a different car in a different image +//Although the robustness of the Car class is not guaranteed! +// +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#include "bot.h" +using namespace cv; + +int main( int argc, char** argv ) +{ + if (argc != 2) { + printf("## You need to supply a number for the image to test ##\n"); + return -1; + } + + //-- Initialize the first car location + Point upperLeft(540,295); + Point lowerRight(683,468); + + //-- Initialize a car object + Bot oddBot(upperLeft,lowerRight); + + //-- Iterate through images + //for (int j = 1; j < 252; j++) + // { + + int j = atoi(argv[1]); + + + char imName1[40]; + char imName2[40]; + sprintf(imName1,"images/test_images/21APR/left%03d.jpg",j); + sprintf(imName2,"images/test_images/21APR/right%03d.jpg",j); + Mat imgL = imread( imName1, CV_LOAD_IMAGE_COLOR ); + Mat imgR = imread( imName2, CV_LOAD_IMAGE_COLOR ); + printf("%s\n",imName1); + if(!imgL.data || !imgR.data) // Check for invalid input + { + printf("## Could not open or find the image ##\n") ; + return -1; + } + //waitKey(10); + //firstCar.updateBoxSize(img1); + + oddBot.updateBoxPos(imgL, imgR); + oddBot.findGoalPos(imgL, imgR); + + waitKey(10); + // } + return 0; +} + diff --git a/src/bot_tracker.cpp~ b/src/bot_tracker.cpp~ new file mode 100644 index 0000000..2aee4d1 --- /dev/null +++ b/src/bot_tracker.cpp~ @@ -0,0 +1,45 @@ +//This file uses the Car class to track a vehicle in a set of images +//Change this file to track a different car in a different image +//Although the robustness of the Car class is not guaranteed! +// +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#include "car.h" +using namespace cv; + +int main( int argc, char** argv ) +{ + //-- Initialize the first car location + Point upperLeft(5,168); + Point lowerRight(53,190); + + Mat initialImg = imread("../car/00000001.jpg", CV_LOAD_IMAGE_GRAYSCALE ); + if(!initialImg.rows) + { + printf("This program expects files in '../car/'\n"); + return 0; + } + //-- Initialize a car object + Car firstCar(upperLeft,lowerRight,initialImg); + + //-- Iterate through images + for (int j = 1; j < 252; j++) + { + char imName1[30]; + char imName2[30]; + sprintf(imName1,"../car/00000%03d.jpg",j); + sprintf(imName2,"../car/00000%03d.jpg",j+1); + Mat img1 = imread( imName1, CV_LOAD_IMAGE_GRAYSCALE ); + Mat img2 = imread( imName2, CV_LOAD_IMAGE_GRAYSCALE ); + printf("%s\n",imName1); + + firstCar.updateBoxSize(img1); + + firstCar.updateBoxPos(img1, img2); + + } + return 0; +} + diff --git a/src/bottracker b/src/bottracker new file mode 100755 index 0000000..460de9d Binary files /dev/null and b/src/bottracker differ diff --git a/src/boundPoints b/src/boundPoints new file mode 100644 index 0000000..94e9139 --- /dev/null +++ b/src/boundPoints @@ -0,0 +1,9 @@ +%YAML:1.0 + boundP: !!opencv-matrix + rows: 4 + cols: 2 + dt: f + data: [ 0.0, 0.0, 0.0, + 10.0, 10.0, 0.0, 10.0, + 10.0] + \ No newline at end of file diff --git a/src/boundPoints.yml b/src/boundPoints.yml new file mode 100644 index 0000000..376bc8f --- /dev/null +++ b/src/boundPoints.yml @@ -0,0 +1,8 @@ +%YAML:1.0 + boundPoints: !!opencv-matrix + rows: 4 + cols: 2 + dt: f + data: [ 0.000000, 0.000000, 0.000000, + 10.000000, 10.000000, 0.000000, 10.000000, + 10.000000] diff --git a/src/boundaryDesc.txt b/src/boundaryDesc.txt new file mode 100644 index 0000000..ee368dc --- /dev/null +++ b/src/boundaryDesc.txt @@ -0,0 +1,26 @@ +Front Left (0,0?) +left LOC +218 164 +right LOC +123 222 + +Scale - 1.8 + + +Front Right (10,0?) +scale - 1.35 +left LOC +1095 110 +right LOC +976 157 + +back Right (10,10?) +scale .7 +left LOC 740 20 +right LOC 671 74 + +back Left (0,10?) +scale .85 +left LOC 245 31 +right LOC 148 97 + diff --git a/src/boundaryDesc.txt~ b/src/boundaryDesc.txt~ new file mode 100644 index 0000000..0414dc5 --- /dev/null +++ b/src/boundaryDesc.txt~ @@ -0,0 +1,21 @@ +Front Left (0,0?) +left LOC +218 164 +right LOC +123 222 + +Scale - 1.8 + + +Front Right (10,0?) +scale - 1.35 +left LOC +1095 110 +right LOC +976 157 + +back Right (10,10?) +scale .7 +left LOC 740 20 +right LOC 671 74 + diff --git a/src/car.cpp b/src/car.cpp new file mode 100644 index 0000000..2491860 --- /dev/null +++ b/src/car.cpp @@ -0,0 +1,484 @@ +//This is the Car class +//The main focus is on two public functions to update the position and size of the tracking box +//The updateBoxSize function can be modified to be more robust for other image sets +// +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#include "car.h" +using namespace cv; + +Car::Car(Point uL, Point lR, Mat initialIm) +{ + + upperLeft = uL; + lowerRight = lR; + inertialTracking = false; + checkBounds(initialIm, &upperLeft, &lowerRight); + templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + updateTemplate = 1; +} + +Car::~Car() +{ +} + +void Car::matchPoints(std::vector* keypointsIn1, std::vector* keypointsIn2,Mat image1, Mat image2, std::vector* keypointsOut1, std::vector* keypointsOut2, int thresh) +{ + if(!keypointsIn1->size() || !keypointsIn2->size()) + { + //-- Detect the keypoints using SURF Detector + int minHessian = 400; + + SurfFeatureDetector detector( minHessian ); + + detector.detect( image1, *keypointsIn1 ); + detector.detect( image2, *keypointsIn2 ); + } + + //-- Extract features for matching + SurfDescriptorExtractor extractor; + + Mat descriptors1, descriptors2; + + extractor.compute(image1, *keypointsIn1, descriptors1); + extractor.compute(image2, *keypointsIn2, descriptors2); + + FlannBasedMatcher matcher; + std::vector matches; + + //-- Match the car SURF points with all the next images SURF points + matcher.match(descriptors1,descriptors2, matches); + + //-- Find match strengths + double maxDist = 0; double minDist = 100; + + for( int i = 0; i < descriptors1.rows; i++ ) + { double dist = matches[i].distance; + if( dist < minDist ) minDist = dist; + if( dist > maxDist ) maxDist = dist; + } + + //-- Keep good matches + std::vector< DMatch > goodMatches; + + for( int i = 0; i < descriptors1.rows; i++ ) + { if( matches[i].distance <= max(thresh*minDist, 0.02) ) + { goodMatches.push_back( matches[i]); } + } + + //-- Fill the good matches keypoint vectors + for ( int i = 0; i < goodMatches.size(); i++) + { + (*keypointsOut1).push_back((*keypointsIn1)[goodMatches[i].queryIdx]); + (*keypointsOut2).push_back((*keypointsIn2)[goodMatches[i].trainIdx]); + } + +} + +void Car::updateBoxPos(Mat image1, Mat image2) +{ + + //-- Detect the keypoints using SURF Detector + int minHessian = 400; + + SurfFeatureDetector detector( minHessian ); + + std::vector keypoints1, keypoints2, keypointsCar1, keypointsGM1, keypointsGM2; + + detector.detect( image1, keypoints1 ); + detector.detect( image2, keypoints2 ); + + Mat imgKeypoints1; Mat imgKeypoints2; + + //-- Detect keypoints that are within the box + + for (int i = 0; i < keypoints1.size(); i++) + { + if (keypoints1[i].pt.x < lowerRight.x && keypoints1[i].pt.x > upperLeft.x && + keypoints1[i].pt.y < lowerRight.y && keypoints1[i].pt.y > upperLeft.y) + { + keypointsCar1.push_back(keypoints1[i]); + } + + } + //-- Match points from within the box to the next frame + matchPoints(&keypointsCar1,&keypoints2,image1,image2,&keypointsGM1,&keypointsGM2,2.1); + + //-- Use the average distance between the pixel coordinates to generate box movement + std::vector xDistance, yDistance; + + //-- Generate the distances + for( int i = 0; i < (int)keypointsGM1.size(); i++ ) + { + xDistance.push_back(keypointsGM2[i].pt.x - keypointsGM1[i].pt.x); + yDistance.push_back(keypointsGM2[i].pt.y - keypointsGM1[i].pt.y); + } + + //-- The actual average distances in x and y + double avgX = std::accumulate(xDistance.begin(), xDistance.end(), 0) / (double)xDistance.size(); + double avgY = std::accumulate(yDistance.begin(), yDistance.end(), 0) / (double)yDistance.size(); + + drawKeypoints( image1, keypointsGM1, imgKeypoints1, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); + + //-- Update the box if we are still tracking the car + if(!inertialTracking && keypointsGM1.size()) + { + rectangle(imgKeypoints1,upperLeft,lowerRight,10,3); + //-- Move the box by the average shift in pixels from this time + upperLeft.x += avgX; + lowerRight.x += avgX; + upperLeft.y += avgY; + lowerRight.y += avgY; + xHistory.push_back(avgX); + yHistory.push_back(avgY); + //-- Make sure the edges of the box are still on the screen + checkBounds(image1, &upperLeft, &lowerRight); + } + else + { + //-- Not sure if the car is in the box + rectangle(imgKeypoints1,upperLeft,lowerRight,100,5); + //-- Move the box by the average shift in pixels historically + double avgXHistorical = std::accumulate(xHistory.begin(), xHistory.end(), 0) / (double)xHistory.size(); + double avgYHistorical = std::accumulate(yHistory.begin(), yHistory.end(), 0) / (double)yHistory.size(); + + upperLeft.x += avgXHistorical; + lowerRight.x += avgXHistorical; + upperLeft.y += avgYHistorical; + lowerRight.y += avgYHistorical; + printf("+++++++++++++++++++ Inertial Tracking - lost car ++++++++++++++++++++++\n"); + //-- Make sure the edges of the box are still on the screen + checkBounds(image1, &upperLeft, &lowerRight); + } + + //-- Show detected (drawn) keypoints + imshow("Bot Tracker", imgKeypoints1 ); + imshow("Template", templateIm); + waitKey(100); + +} + +void Car::getTemplateMatch(Mat image, Point* matchUL, Point* matchLR, Mat* templateImage) +{ + //-- This function will take a template image, resize it to the box size, and find it in the image + //-- This is used as a check on the SURF features update of the box and the edge resize of the box + + //-- If the template image doesn't exist, make one + if(!templateImage->rows) + { + checkBounds(image,&upperLeft,&lowerRight); + *templateImage = image(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + } + + Mat result; + int resultCols = image.cols - templateImage->cols + 1; + int resultRows = image.rows - templateImage->rows + 1; + double minVal; double maxVal; Point minLoc; Point maxLoc; + + result.create( resultCols, resultRows, CV_32FC1 ); + + //-- Resize the template for the current box size + Mat resizedTemplate; + resize(*templateImage,resizedTemplate,Size( lowerRight.x - upperLeft.x,lowerRight.y - upperLeft.y)); + //-- Match and normalize + matchTemplate(image, resizedTemplate, result, CV_TM_SQDIFF_NORMED ); + normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() ); + + //-- Grab the best match from the result matrix + minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() ); + *matchUL = minLoc; + + //-- Set the other match coordinates + matchLR->x = matchUL->x + resizedTemplate.cols; + matchLR->y = matchUL->y + resizedTemplate.rows; +} + +void Car::updateBoxSize(Mat image) +{ + //-- This function uses a sparse set of edges to resize the box describing the car + //-- This assumes there are many edges on the car, and space between the car and edges in the world + //-- This will drop small edges on the boundaries, to account for edges like the road that are always near the car + + //-- Create an image using Canny edge detection + Mat subImage,edges,subEdge; + + blur(image, edges, Size(5,5) ); + + Canny(edges,edges,79,102,3); + subEdge = edges(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + + //-- Get the current sum of gradients to compare against + float currentGradient = sum(subEdge)[0]; + + //-- This gives us a history of gradient sums to use to check if the current block is similar to the last block + //-- This did not provide enough information to spend time implementing + gradientHistory.push_back((int)currentGradient/255); + double gAvg = std::accumulate(gradientHistory.begin(), gradientHistory.end(), 0) / (double)gradientHistory.size(); + + //-- Expand the box and see if the increase in captured edges is large enough + /* + for (int i=0;i<4;i++) + { + Point newUL(upperLeft.x-2*i,upperLeft.y-2*i); + Point newLR(lowerRight.x+2*i,lowerRight.y+2*i); + checkBounds(edges,&newUL,&newLR); + subEdge = edges(Range(newUL.y,newLR.y),Range(newUL.x,newLR.x)); + //-- If there are enough extra edges, keep the new box size + if (sum(subEdge)[0]/255 > currentGradient/255 + 25) + { + upperLeft = newUL; + lowerRight = newLR; + } + } + + //-- The box has been greedily expanded, now contract it until there is more than noise on the edges + int noiseThreshold = 8; + while(sum(edges(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x-1,upperLeft.x+1)))[0]/255 < noiseThreshold && + upperLeft.x < lowerRight.x - 50) + { + upperLeft.x += 1; + } + + while(sum(edges(Range(upperLeft.y-1,upperLeft.y+1),Range(upperLeft.x,lowerRight.x)))[0]/255 < noiseThreshold && + upperLeft.y < lowerRight.y - 30) + { + upperLeft.y += 1; + } + + while(sum(edges(Range(upperLeft.y,lowerRight.y),Range(lowerRight.x-1,lowerRight.x+1)))[0]/255 < noiseThreshold && + upperLeft.x < lowerRight.x - 50) + { + lowerRight.x -= 1; + } + + while(sum(edges(Range(lowerRight.y-1,lowerRight.y+1),Range(upperLeft.x,lowerRight.x)))[0]/255 < noiseThreshold && + upperLeft.y < lowerRight.y - 30) + { + lowerRight.y -= 1; + } + + */ + checkBounds(edges,&upperLeft,&lowerRight); + rectangle(edges,upperLeft,lowerRight,150); + + Point matchUL, matchLR; + + //-- Use the current template image to search for the car in the image + getTemplateMatch(image,&matchUL,&matchLR,&templateIm); + + checkBounds(image, &matchUL, &matchLR); + + rectangle( edges, matchUL, matchLR, 100, 2, 8, 0 ); + + //-- Allow for error in template matching, up to the overlap factor + double xOverlap,yOverlap,overlapFactor; + //-- This will consider success anything more than 50% overlap + overlapFactor = 0.5; + xOverlap = (lowerRight.x - upperLeft.x)*overlapFactor; + yOverlap = (lowerRight.y - upperLeft.y)*overlapFactor; + + if((matchUL.x < upperLeft.x + xOverlap && + matchUL.x > upperLeft.x - xOverlap && + matchUL.y < upperLeft.y + yOverlap && + matchUL.y > upperLeft.y - yOverlap) ) + { + upperLeft = matchUL; + lowerRight = matchLR; + //-- The template matches the current box and it has been 20 frames since we updated the template + if(updateTemplate==50) + { + templateIm = image(Range(matchUL.y,matchLR.y),Range(matchUL.x,matchLR.x)); updateTemplate = 0; + printf("------------------- Updated template --------------------\n"); + } + else updateTemplate++; + //-- We are tracking the car + inertialTracking = false; + } + else + { + //-- The template isn't matching the box + inertialTracking = true; + } + + //imshow("Canny Edge Detector",edges); + waitKey(10); +} + +void Car::checkBounds(Mat image, Point* uL, Point* lR) +{ + //If any point doesn't exist, set it to 0 + if(!uL->x) uL->x = 0; + if(!uL->y) uL->y = 0; + if(!lR->x) lR->x = 0; + if(!lR->y) lR->y = 0; + + //Fix the box points if they are outside of bounds + //The smallest box is 50,30 + if (uL->x < 1) uL->x = 1; + if (uL->x > image.size().width) uL->x = image.size().width - 52; + if (lR->x < uL->x + 50) lR->x = uL->x + 50; + if (lR->x > image.size().width) lR->x = image.size().width - 1; + + + if (uL->y < 1) uL->y = 1; + if (uL->y > image.size().height) uL->y = image.size().height - 32; + if (lR->y < uL->y + 30) lR->y = uL->y + 30; + if (lR->y > image.size().height) lR->y = image.size().height - 1; + +} + + +bool Car::unitTest(Mat image) +{ + bool allTests = true; + //-- This function is a simple example of unit testing + //-- There are frameworks that do this better + + Point matchUL,matchLR; + + //-- First test checkBounds + //-- This function is a check used throughout the class + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Empty Points\n"); + + matchUL.x = 0; + matchUL.y = 0; + matchLR.x = 0; + matchLR.y = 0; + + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Zero Points\n"); + + matchUL.x = image.size().width*2; + matchUL.y = image.size().height*2; + matchLR.x = image.size().width*2; + matchLR.y = image.size().height*2; + + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Out of Bounds Points\n"); + + matchUL.x = image.size().width - 10; + matchUL.y = image.size().height - 10; + matchLR.x = 2; + matchLR.y = 2; + + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Reversed Points\n"); + + if(templateIm.rows) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" Constructor Template Doesn't Exist\n"); + + //-- Test getTemplateMatch + Mat emptyTemplate; + getTemplateMatch(image,&matchUL,&matchLR,&emptyTemplate); + if(matchUL== upperLeft) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" Empty Template Doesn't Match\n"); + + getTemplateMatch(image, &matchUL, &matchLR, &templateIm); + if(matchUL== upperLeft) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" Constructor Template Doesn't Match\n"); + + //-- Test matchPoints + std::vector i1,i2,o1,o2; + matchPoints(&i1,&i2,image,image,&o1,&o2,5); + if(o1.size()) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" matchPoints Empty Keypoints\n"); + + if (o1[0].pt.x == o2[0].pt.x && o1[0].pt.y == o2[0].pt.y) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" matchPoints Detector Identical Image\n"); + //-- Test updateBoxPos + matchUL = upperLeft; + matchLR = lowerRight; + updateBoxPos(image,image); + if(matchUL.x == upperLeft.x) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" updateBoxPos Identical Image 0 Movement\n"); + + //-- Test updateBoxSize - check that the box is about the same size + matchUL = upperLeft; + matchLR = lowerRight; + updateBoxSize(image); + if(matchUL.x < upperLeft.x + 5 && + matchUL.x > upperLeft.x - 5 && + matchUL.y < upperLeft.y + 5 && + matchUL.y > upperLeft.y - 5 && + matchLR.x < lowerRight.x + 5 && + matchLR.x > lowerRight.x - 5 && + matchLR.x < lowerRight.x + 5 && + matchLR.x > lowerRight.x - 5) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" updateBoxSize Identical Image\n"); + + return allTests; +} diff --git a/src/car.cpp~ b/src/car.cpp~ new file mode 100644 index 0000000..338b399 --- /dev/null +++ b/src/car.cpp~ @@ -0,0 +1,481 @@ +//This is the Car class +//The main focus is on two public functions to update the position and size of the tracking box +//The updateBoxSize function can be modified to be more robust for other image sets +// +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#include "car.h" +using namespace cv; + +Car::Car(Point uL, Point lR, Mat initialIm) +{ + + upperLeft = uL; + lowerRight = lR; + inertialTracking = false; + checkBounds(initialIm, &upperLeft, &lowerRight); + templateIm = initialIm(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + updateTemplate = 1; +} + +Car::~Car() +{ +} + +void Car::matchPoints(std::vector* keypointsIn1, std::vector* keypointsIn2,Mat image1, Mat image2, std::vector* keypointsOut1, std::vector* keypointsOut2, int thresh) +{ + if(!keypointsIn1->size() || !keypointsIn2->size()) + { + //-- Detect the keypoints using SURF Detector + int minHessian = 400; + + SurfFeatureDetector detector( minHessian ); + + detector.detect( image1, *keypointsIn1 ); + detector.detect( image2, *keypointsIn2 ); + } + + //-- Extract features for matching + SurfDescriptorExtractor extractor; + + Mat descriptors1, descriptors2; + + extractor.compute(image1, *keypointsIn1, descriptors1); + extractor.compute(image2, *keypointsIn2, descriptors2); + + FlannBasedMatcher matcher; + std::vector matches; + + //-- Match the car SURF points with all the next images SURF points + matcher.match(descriptors1,descriptors2, matches); + + //-- Find match strengths + double maxDist = 0; double minDist = 100; + + for( int i = 0; i < descriptors1.rows; i++ ) + { double dist = matches[i].distance; + if( dist < minDist ) minDist = dist; + if( dist > maxDist ) maxDist = dist; + } + + //-- Keep good matches + std::vector< DMatch > goodMatches; + + for( int i = 0; i < descriptors1.rows; i++ ) + { if( matches[i].distance <= max(thresh*minDist, 0.02) ) + { goodMatches.push_back( matches[i]); } + } + + //-- Fill the good matches keypoint vectors + for ( int i = 0; i < goodMatches.size(); i++) + { + (*keypointsOut1).push_back((*keypointsIn1)[goodMatches[i].queryIdx]); + (*keypointsOut2).push_back((*keypointsIn2)[goodMatches[i].trainIdx]); + } + +} + +void Car::updateBoxPos(Mat image1, Mat image2) +{ + + //-- Detect the keypoints using SURF Detector + int minHessian = 400; + + SurfFeatureDetector detector( minHessian ); + + std::vector keypoints1, keypoints2, keypointsCar1, keypointsGM1, keypointsGM2; + + detector.detect( image1, keypoints1 ); + detector.detect( image2, keypoints2 ); + + Mat imgKeypoints1; Mat imgKeypoints2; + + //-- Detect keypoints that are within the box + + for (int i = 0; i < keypoints1.size(); i++) + { + if (keypoints1[i].pt.x < lowerRight.x && keypoints1[i].pt.x > upperLeft.x && + keypoints1[i].pt.y < lowerRight.y && keypoints1[i].pt.y > upperLeft.y) + { + keypointsCar1.push_back(keypoints1[i]); + } + + } + //-- Match points from within the box to the next frame + matchPoints(&keypointsCar1,&keypoints2,image1,image2,&keypointsGM1,&keypointsGM2,2.1); + + //-- Use the average distance between the pixel coordinates to generate box movement + std::vector xDistance, yDistance; + + //-- Generate the distances + for( int i = 0; i < (int)keypointsGM1.size(); i++ ) + { + xDistance.push_back(keypointsGM2[i].pt.x - keypointsGM1[i].pt.x); + yDistance.push_back(keypointsGM2[i].pt.y - keypointsGM1[i].pt.y); + } + + //-- The actual average distances in x and y + double avgX = std::accumulate(xDistance.begin(), xDistance.end(), 0) / (double)xDistance.size(); + double avgY = std::accumulate(yDistance.begin(), yDistance.end(), 0) / (double)yDistance.size(); + + drawKeypoints( image1, keypointsGM1, imgKeypoints1, Scalar::all(-1), DrawMatchesFlags::DEFAULT ); + + //-- Update the box if we are still tracking the car + if(!inertialTracking && keypointsGM1.size()) + { + rectangle(imgKeypoints1,upperLeft,lowerRight,10,3); + //-- Move the box by the average shift in pixels from this time + upperLeft.x += avgX; + lowerRight.x += avgX; + upperLeft.y += avgY; + lowerRight.y += avgY; + xHistory.push_back(avgX); + yHistory.push_back(avgY); + //-- Make sure the edges of the box are still on the screen + checkBounds(image1, &upperLeft, &lowerRight); + } + else + { + //-- Not sure if the car is in the box + rectangle(imgKeypoints1,upperLeft,lowerRight,100,5); + //-- Move the box by the average shift in pixels historically + double avgXHistorical = std::accumulate(xHistory.begin(), xHistory.end(), 0) / (double)xHistory.size(); + double avgYHistorical = std::accumulate(yHistory.begin(), yHistory.end(), 0) / (double)yHistory.size(); + + upperLeft.x += avgXHistorical; + lowerRight.x += avgXHistorical; + upperLeft.y += avgYHistorical; + lowerRight.y += avgYHistorical; + printf("+++++++++++++++++++ Inertial Tracking - lost car ++++++++++++++++++++++\n"); + //-- Make sure the edges of the box are still on the screen + checkBounds(image1, &upperLeft, &lowerRight); + } + + //-- Show detected (drawn) keypoints + imshow("Car Tracker", imgKeypoints1 ); + + waitKey(10); + +} + +void Car::getTemplateMatch(Mat image, Point* matchUL, Point* matchLR, Mat* templateImage) +{ + //-- This function will take a template image, resize it to the box size, and find it in the image + //-- This is used as a check on the SURF features update of the box and the edge resize of the box + + //-- If the template image doesn't exist, make one + if(!templateImage->rows) + { + checkBounds(image,&upperLeft,&lowerRight); + *templateImage = image(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + } + + Mat result; + int resultCols = image.cols - templateImage->cols + 1; + int resultRows = image.rows - templateImage->rows + 1; + double minVal; double maxVal; Point minLoc; Point maxLoc; + + result.create( resultCols, resultRows, CV_32FC1 ); + + //-- Resize the template for the current box size + Mat resizedTemplate; + resize(*templateImage,resizedTemplate,Size( lowerRight.x - upperLeft.x,lowerRight.y - upperLeft.y)); + //-- Match and normalize + matchTemplate(image, resizedTemplate, result, CV_TM_SQDIFF_NORMED ); + normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() ); + + //-- Grab the best match from the result matrix + minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() ); + *matchUL = minLoc; + + //-- Set the other match coordinates + matchLR->x = matchUL->x + resizedTemplate.cols; + matchLR->y = matchUL->y + resizedTemplate.rows; +} + +void Car::updateBoxSize(Mat image) +{ + //-- This function uses a sparse set of edges to resize the box describing the car + //-- This assumes there are many edges on the car, and space between the car and edges in the world + //-- This will drop small edges on the boundaries, to account for edges like the road that are always near the car + + //-- Create an image using Canny edge detection + Mat subImage,edges,subEdge; + + blur(image, edges, Size(5,5) ); + + Canny(edges,edges,79,102,3); + subEdge = edges(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); + + //-- Get the current sum of gradients to compare against + float currentGradient = sum(subEdge)[0]; + + //-- This gives us a history of gradient sums to use to check if the current block is similar to the last block + //-- This did not provide enough information to spend time implementing + gradientHistory.push_back((int)currentGradient/255); + double gAvg = std::accumulate(gradientHistory.begin(), gradientHistory.end(), 0) / (double)gradientHistory.size(); + + //-- Expand the box and see if the increase in captured edges is large enough + for (int i=0;i<4;i++) + { + Point newUL(upperLeft.x-2*i,upperLeft.y-2*i); + Point newLR(lowerRight.x+2*i,lowerRight.y+2*i); + checkBounds(edges,&newUL,&newLR); + subEdge = edges(Range(newUL.y,newLR.y),Range(newUL.x,newLR.x)); + //-- If there are enough extra edges, keep the new box size + if (sum(subEdge)[0]/255 > currentGradient/255 + 25) + { + upperLeft = newUL; + lowerRight = newLR; + } + } + + //-- The box has been greedily expanded, now contract it until there is more than noise on the edges + int noiseThreshold = 8; + while(sum(edges(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x-1,upperLeft.x+1)))[0]/255 < noiseThreshold && + upperLeft.x < lowerRight.x - 50) + { + upperLeft.x += 1; + } + + while(sum(edges(Range(upperLeft.y-1,upperLeft.y+1),Range(upperLeft.x,lowerRight.x)))[0]/255 < noiseThreshold && + upperLeft.y < lowerRight.y - 30) + { + upperLeft.y += 1; + } + + while(sum(edges(Range(upperLeft.y,lowerRight.y),Range(lowerRight.x-1,lowerRight.x+1)))[0]/255 < noiseThreshold && + upperLeft.x < lowerRight.x - 50) + { + lowerRight.x -= 1; + } + + while(sum(edges(Range(lowerRight.y-1,lowerRight.y+1),Range(upperLeft.x,lowerRight.x)))[0]/255 < noiseThreshold && + upperLeft.y < lowerRight.y - 30) + { + lowerRight.y -= 1; + } + + + checkBounds(edges,&upperLeft,&lowerRight); + rectangle(edges,upperLeft,lowerRight,150); + + Point matchUL, matchLR; + + //-- Use the current template image to search for the car in the image + getTemplateMatch(image,&matchUL,&matchLR,&templateIm); + + checkBounds(image, &matchUL, &matchLR); + + rectangle( edges, matchUL, matchLR, 100, 2, 8, 0 ); + + //-- Allow for error in template matching, up to the overlap factor + double xOverlap,yOverlap,overlapFactor; + //-- This will consider success anything more than 50% overlap + overlapFactor = 0.5; + xOverlap = (lowerRight.x - upperLeft.x)*overlapFactor; + yOverlap = (lowerRight.y - upperLeft.y)*overlapFactor; + + if((matchUL.x < upperLeft.x + xOverlap && + matchUL.x > upperLeft.x - xOverlap && + matchUL.y < upperLeft.y + yOverlap && + matchUL.y > upperLeft.y - yOverlap) ) + { + //-- The template matches the current box and it has been 20 frames since we updated the template + if(updateTemplate==15) + { + templateIm = image(Range(upperLeft.y,lowerRight.y),Range(upperLeft.x,lowerRight.x)); updateTemplate = 0; + printf("------------------- Updated template --------------------\n"); + } + else updateTemplate++; + //-- We are tracking the car + inertialTracking = false; + } + else + { + //-- The template isn't matching the box + inertialTracking = true; + } + + imshow("Canny Edge Detector",edges); + waitKey(10); +} + +void Car::checkBounds(Mat image, Point* uL, Point* lR) +{ + //If any point doesn't exist, set it to 0 + if(!uL->x) uL->x = 0; + if(!uL->y) uL->y = 0; + if(!lR->x) lR->x = 0; + if(!lR->y) lR->y = 0; + + //Fix the box points if they are outside of bounds + //The smallest box is 50,30 + if (uL->x < 1) uL->x = 1; + if (uL->x > image.size().width) uL->x = image.size().width - 52; + if (lR->x < uL->x + 50) lR->x = uL->x + 50; + if (lR->x > image.size().width) lR->x = image.size().width - 1; + + + if (uL->y < 1) uL->y = 1; + if (uL->y > image.size().height) uL->y = image.size().height - 32; + if (lR->y < uL->y + 30) lR->y = uL->y + 30; + if (lR->y > image.size().height) lR->y = image.size().height - 1; + +} + + +bool Car::unitTest(Mat image) +{ + bool allTests = true; + //-- This function is a simple example of unit testing + //-- There are frameworks that do this better + + Point matchUL,matchLR; + + //-- First test checkBounds + //-- This function is a check used throughout the class + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Empty Points\n"); + + matchUL.x = 0; + matchUL.y = 0; + matchLR.x = 0; + matchLR.y = 0; + + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Zero Points\n"); + + matchUL.x = image.size().width*2; + matchUL.y = image.size().height*2; + matchLR.x = image.size().width*2; + matchLR.y = image.size().height*2; + + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Out of Bounds Points\n"); + + matchUL.x = image.size().width - 10; + matchUL.y = image.size().height - 10; + matchLR.x = 2; + matchLR.y = 2; + + checkBounds(image, &matchUL, &matchLR); + if(matchUL.x > 0 && matchUL.x < image.size().width && + matchUL.y > 0 && matchUL.y < image.size().height && + matchLR.x > 0 && matchLR.x < image.size().width && + matchLR.y > 0 && matchLR.y < image.size().height) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" checkBounds Reversed Points\n"); + + if(templateIm.rows) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" Constructor Template Doesn't Exist\n"); + + //-- Test getTemplateMatch + Mat emptyTemplate; + getTemplateMatch(image,&matchUL,&matchLR,&emptyTemplate); + if(matchUL== upperLeft) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" Empty Template Doesn't Match\n"); + + getTemplateMatch(image, &matchUL, &matchLR, &templateIm); + if(matchUL== upperLeft) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" Constructor Template Doesn't Match\n"); + + //-- Test matchPoints + std::vector i1,i2,o1,o2; + matchPoints(&i1,&i2,image,image,&o1,&o2,5); + if(o1.size()) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" matchPoints Empty Keypoints\n"); + + if (o1[0].pt.x == o2[0].pt.x && o1[0].pt.y == o2[0].pt.y) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" matchPoints Detector Identical Image\n"); + //-- Test updateBoxPos + matchUL = upperLeft; + matchLR = lowerRight; + updateBoxPos(image,image); + if(matchUL.x == upperLeft.x) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" updateBoxPos Identical Image 0 Movement\n"); + + //-- Test updateBoxSize - check that the box is about the same size + matchUL = upperLeft; + matchLR = lowerRight; + updateBoxSize(image); + if(matchUL.x < upperLeft.x + 5 && + matchUL.x > upperLeft.x - 5 && + matchUL.y < upperLeft.y + 5 && + matchUL.y > upperLeft.y - 5 && + matchLR.x < lowerRight.x + 5 && + matchLR.x > lowerRight.x - 5 && + matchLR.x < lowerRight.x + 5 && + matchLR.x > lowerRight.x - 5) printf("[ PASS ]"); + else + { + printf("[ FAIL ]"); + allTests = false; + } + printf(" updateBoxSize Identical Image\n"); + + return allTests; +} diff --git a/src/car.h b/src/car.h new file mode 100644 index 0000000..50c6d68 --- /dev/null +++ b/src/car.h @@ -0,0 +1,54 @@ +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#ifndef CAR_H +#define CAR_H + + +#include +#include +#include +#include +#include +#include +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/core/core.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/nonfree/nonfree.hpp" + +using namespace cv; + +class Car +{ + public: + Car(Point uL, Point lR, Mat initialIm); + ~Car(); + + void updateBoxPos(Mat image1, Mat image2); + void updateBoxSize(Mat image); + bool unitTest(Mat image); + + private: + Point upperLeft; + Point lowerRight; + bool inertialTracking; + Mat templateIm; + int updateTemplate; + + std::vector confirmedFeatures; + std::vector gradientHistory; + std::vector xHistory; + std::vector yHistory; + + void checkBounds(Mat image, Point* uL, Point* lR); + void getTemplateMatch(Mat image, Point* matchUL, Point* matchLR, Mat* templateImage); + void matchPoints(std::vector* keypointsIn1, std::vector* keypointsIn2,Mat image1, Mat image2, std::vector* keypointsOut1, std::vector* keypointsOut2, int thresh); + + +}; + + +#endif diff --git a/src/deprecatedbot.h b/src/deprecatedbot.h new file mode 100644 index 0000000..0f7a28b --- /dev/null +++ b/src/deprecatedbot.h @@ -0,0 +1,58 @@ +//Written by Eric Danziger +//04 March 2015 +//ericdanziger@cmu.edu + +#ifndef BOT_H +#define BOT_H + + +#include +#include +#include +#include +#include +#include +#include +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/core/core.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/nonfree/nonfree.hpp" +#include "opencv2/calib3d/calib3d.hpp" + +using namespace cv; + +class Bot +{ + public: + Bot(Point uL, Point lR); + ~Bot(); + + void updateBoxPos(Mat image1, Mat image2); + void findGoalPos(Mat image1, Mat image2); + void updateBoxSize(Mat image); + bool unitTest(Mat image); + + private: + Point upperLeft; + Point lowerRight; + bool inertialTracking; + Mat templateIm; + int updateTemplate; + + std::vector confirmedFeatures; + std::vector gradientHistory; + std::vector xHistory; + std::vector yHistory; + + void checkBounds(Mat image, Point* uL, Point* lR); + void getTemplateMatch(Mat image, Point* matchUL, Point* matchLR, Mat* templateImage, double* strongestResponse); + void matchPoints(std::vector* keypointsIn1, std::vector* keypointsIn2,Mat image1, Mat image2, std::vector* keypointsOut1, std::vector* keypointsOut2, int thresh); + + + +}; + + +#endif diff --git a/src/estimateOfFourCorners.txt b/src/estimateOfFourCorners.txt new file mode 100644 index 0000000..62f2b9c --- /dev/null +++ b/src/estimateOfFourCorners.txt @@ -0,0 +1,10 @@ +back left +-14.7,39.7,4737 +front left +7.14,40.3,4800.57 +back right +-43.5,72.5,6245 +front right +-20.7,64,5519.7 + + diff --git a/src/estimateTriangulation.m b/src/estimateTriangulation.m new file mode 100644 index 0000000..46a40f4 --- /dev/null +++ b/src/estimateTriangulation.m @@ -0,0 +1,48 @@ +imLeft = im2double(rgb2gray(imread('images/left012.jpg'))); +imRight = im2double(rgb2gray(imread('images/right012.jpg'))); +rect = [540 295 683 468]; +load('template.mat'); +H=vision.TemplateMatcher; + +close all; +pause(.001); + + + +imshow(imLeft); +hold on; +%find template +LOC = step(H,imLeft,template); + +height = rect(4) - rect(2); +width = rect(3) - rect(1); + +plot(LOC(1)-width/2,LOC(2)-height/2,'bO'); +rectangle('Position',[LOC(1) - width/2,LOC(2) - height/2,width,height],'EdgeColor','blue'); + +hold off; +pause(.7); + +close all; + +R = stereoParams.RotationOfCamera2; +t = stereoParams.TranslationOfCamera2'; +Rt = [R t]; +[worldPoints,matchedFeatures,pts1,pts2,sp1] = triangulateIm(imLeft,imRight, stereoParams.CameraParameters1.IntrinsicMatrix, stereoParams.CameraParameters2.IntrinsicMatrix,Rt); +onBoxPts = []; +for i=1:length(pts1) + if(pts1(1,i) > rect(1) && pts1(1,i) < rect(3) && pts1(2,i) > rect(2) && pts1(2,i) < rect(4)) + worldPoints(:,i) + onBoxPts = [onBoxPts i]; + end +end +imshow(imLeft); +hold on; +plot(pts1(1,onBoxPts),pts1(2,onBoxPts),'rO') +hold off; + +figure; +imshow(imRight); +hold on; +plot(pts2(1,onBoxPts),pts2(2,onBoxPts),'bO') +hold off; \ No newline at end of file diff --git a/src/global_controller.cpp b/src/global_controller.cpp new file mode 100644 index 0000000..8257d59 --- /dev/null +++ b/src/global_controller.cpp @@ -0,0 +1,202 @@ +/* + * Global controller to follow a trajectory + * + * By: Christopher Dunkers, cmdunkers@cmu.edu + * March 1, 2015 + */ + + +#include "trajectory_controller/global_controller.h" + +/********************************************* + * + * + *********************************************/ +double getValueFromPolynomial(std::vector alphas, double time){ + + double value = 0.0; + for(int i = 0; i < alphas.size(); i++){ + value += alphas[i] * pow(time,i); + } + return value; +} + +/********************************************* + * + * + *********************************************/ +global_controller::global_controller(){ + + //the main node handle + ros::NodeHandle nh; + + //grab the parameters + ros::NodeHandle private_node_handle_("~"); + + //param + //distance step for resolution + + plan_pub = nh.advertise("/plan", 1000, true); + waypoints_sub = nh.subscribe("/waypoints", 1000, &global_controller::update_waypoints, this); + + step = 0.05; + + //set up the current robot pose + cur_robot_pose.header.stamp = ros::Time::now(); + cur_robot_pose.header.frame_id = "/world"; + + cur_robot_pose.pose.position.x = 0.0; + cur_robot_pose.pose.position.y = 0.0; + + tf::Quaternion goal_quat = tf::createQuaternionFromYaw(0.0); + cur_robot_pose.pose.orientation.x = goal_quat.x(); + cur_robot_pose.pose.orientation.y = goal_quat.y(); + cur_robot_pose.pose.orientation.z = goal_quat.z(); + cur_robot_pose.pose.orientation.w = goal_quat.w(); + +} + +/********************************************* + * + * + *********************************************/ +void global_controller::update_waypoints(const nav_msgs::Path::ConstPtr& msg){ + bool goal_changed = false; + +// if(goal_waypoints.size() == msg->poses.size()){ +// for(int i = 0; i < msg->poses.size(); i++){ +// if(comparePoses(goal_waypoints.poses[i].pose, msg->poses[i].pose){ +// goal_changed = true; +// break; +// } +// } +// } else { +// goal_changed = true; +// } + + goal_changed = true; + + //update path if goal position changed + if(goal_changed){ + goal_waypoints.poses.clear(); + goal_waypoints = *msg; + this->create_path(); + } +} + +/********************************************* + * + * + *********************************************/ +void global_controller::create_path(){ + //find and store the robots current pose in the world frame (using tf) + try{ + tf::StampedTransform transform; + tf_listener.lookupTransform("/base_link", "/world", ros::Time(0), transform); + cur_robot_pose.pose.position.x = transform.getOrigin().x(); + cur_robot_pose.pose.position.y = transform.getOrigin().y(); + + tf::Quaternion goal_quat = tf::createQuaternionFromYaw(tf::getYaw(transform.getRotation())); + cur_robot_pose.pose.orientation.x = goal_quat.x(); + cur_robot_pose.pose.orientation.y = goal_quat.y(); + cur_robot_pose.pose.orientation.z = goal_quat.z(); + cur_robot_pose.pose.orientation.w = goal_quat.w(); + } catch (tf::TransformException ex){ + ROS_WARN("%s",ex.what()); + //ROS_ERROR("%s",ex.what()); + //ros::Duration(1.0).sleep(); + } + + ROS_INFO("checked for new robot position"); + + //calc the polynomial for the two points + // assume straight line + // assume heading is that from the first to second point + x_alphas.clear(); + y_alphas.clear(); + h_alphas.clear(); + + double x_start = cur_robot_pose.pose.position.x; + double y_start = cur_robot_pose.pose.position.y; + + double x_end = goal_waypoints.poses.front().pose.position.x; + double y_end = goal_waypoints.poses.front().pose.position.y; + + ROS_INFO("x_start %f", x_start); + ROS_INFO("x_end %f", x_end); + ROS_INFO("y_start %f", y_start); + ROS_INFO("y_end %f", y_end); + + std::vector x_alpha_1, y_alpha_1, h_alpha_1; + x_alpha_1.push_back(x_start); + x_alpha_1.push_back(x_end - x_start); + x_alphas.push_back(x_alpha_1); + y_alpha_1.push_back(y_start); + y_alpha_1.push_back(y_end - y_start); + y_alphas.push_back(y_alpha_1); + h_alpha_1.push_back(atan2(y_end - y_start,x_end - x_start)); + h_alphas.push_back(h_alpha_1); + ROS_INFO("made the alphas"); + //calculate the new path + plan.poses.clear(); + + ros::Time plan_time = ros::Time::now(); + for(int i = 0; i < x_alphas.size(); i++){ + double distance = sqrt(pow(x_alphas[i].back(),2) + pow(y_alphas[i].back(),2)); + double time_increment = 1.0/double(floor(distance/this->step)); + ROS_INFO("distance %f time interval %f", distance,time_increment); + ROS_INFO("Created the Path for first polynomial"); + + for(double t = 0.0; t < 1.0; t += time_increment){ + geometry_msgs::PoseStamped next_pose; + next_pose.header.stamp = plan_time; + next_pose.header.frame_id = + + next_pose.pose.position.x = getValueFromPolynomial(x_alphas[i],t); + next_pose.pose.position.y = getValueFromPolynomial(y_alphas[i],t); + + tf::Quaternion goal_quat = tf::createQuaternionFromYaw(getValueFromPolynomial(h_alphas[i],t)); + next_pose.pose.orientation.x = goal_quat.x(); + next_pose.pose.orientation.y = goal_quat.y(); + next_pose.pose.orientation.z = goal_quat.z(); + next_pose.pose.orientation.w = goal_quat.w(); + + plan.poses.push_back(next_pose); + //ROS_INFO("Created polynomial at time %f ", t); + } + //add the goal to the end of the path + plan.poses.push_back(goal_waypoints.poses[i]); + ROS_INFO("Created path for polynomial %d", i); + } + ROS_INFO("Created the Path and Publishing"); + //publish the new path + plan_pub.publish(plan); + +} + +/********************************************* + * + * + *********************************************/ +int main(int argc, char **argv){ + + ros::init(argc, argv, "global_controller"); + + global_controller gc; + + ROS_INFO("global path planner started!"); + + ros::Rate loop_rate(10); + + while (ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } + + return 0; + +} + + + diff --git a/src/images/.DS_Store b/src/images/.DS_Store new file mode 100644 index 0000000..d9c5543 Binary files /dev/null and b/src/images/.DS_Store differ diff --git a/src/images/estimateTriangulation b/src/images/estimateTriangulation new file mode 100644 index 0000000..ab49172 --- /dev/null +++ b/src/images/estimateTriangulation @@ -0,0 +1,4 @@ +imLeft = imread('left012.jpg'); +imRight = imread('right012.jpg'); + +[worldPoints,matchedFeatures,pts1,pts2,sp1] = triangulateIm(imLeft,imLeft, stereoParams.CameraParameters1.IntrinsicMatrix, stereoParams.CameraParameters2.IntrinsicMatrix); diff --git a/src/images/estimateTriangulation.m b/src/images/estimateTriangulation.m new file mode 100644 index 0000000..c037501 --- /dev/null +++ b/src/images/estimateTriangulation.m @@ -0,0 +1,4 @@ +imLeft = double(rgb2gray(imread('left012.jpg'))); +imRight = double(rgb2gray(imread('right012.jpg'))); + +[worldPoints,matchedFeatures,pts1,pts2,sp1] = triangulateIm(imLeft,imRight, stereoParams.CameraParameters1.IntrinsicMatrix, stereoParams.CameraParameters2.IntrinsicMatrix); diff --git a/src/images/rightCameraParams.mat b/src/images/rightCameraParams.mat new file mode 100644 index 0000000..c89b618 Binary files /dev/null and b/src/images/rightCameraParams.mat differ diff --git a/src/images/rotation_images/.DS_Store b/src/images/rotation_images/.DS_Store new file mode 100644 index 0000000..5008ddf Binary files /dev/null and b/src/images/rotation_images/.DS_Store differ diff --git a/src/images/rotation_images/20APR/left001.jpg b/src/images/rotation_images/20APR/left001.jpg new file mode 100644 index 0000000..a79f882 Binary files /dev/null and b/src/images/rotation_images/20APR/left001.jpg differ diff --git a/src/images/rotation_images/20APR/left002.jpg b/src/images/rotation_images/20APR/left002.jpg new file mode 100644 index 0000000..1d4a49f Binary files /dev/null and b/src/images/rotation_images/20APR/left002.jpg differ diff --git a/src/images/rotation_images/20APR/left003.jpg b/src/images/rotation_images/20APR/left003.jpg new file mode 100644 index 0000000..e5ba573 Binary files /dev/null and b/src/images/rotation_images/20APR/left003.jpg differ diff --git a/src/images/rotation_images/20APR/left004.jpg b/src/images/rotation_images/20APR/left004.jpg new file mode 100644 index 0000000..9953c1e Binary files /dev/null and b/src/images/rotation_images/20APR/left004.jpg differ diff --git a/src/images/rotation_images/20APR/left005.jpg b/src/images/rotation_images/20APR/left005.jpg new file mode 100644 index 0000000..015555f Binary files /dev/null and b/src/images/rotation_images/20APR/left005.jpg differ diff --git a/src/images/rotation_images/20APR/left006.jpg b/src/images/rotation_images/20APR/left006.jpg new file mode 100644 index 0000000..741a20c Binary files /dev/null and b/src/images/rotation_images/20APR/left006.jpg differ diff --git a/src/images/rotation_images/20APR/left007.jpg b/src/images/rotation_images/20APR/left007.jpg new file mode 100644 index 0000000..9dab9f2 Binary files /dev/null and b/src/images/rotation_images/20APR/left007.jpg differ diff --git a/src/images/rotation_images/20APR/left008.jpg b/src/images/rotation_images/20APR/left008.jpg new file mode 100644 index 0000000..0650b41 Binary files /dev/null and b/src/images/rotation_images/20APR/left008.jpg differ diff --git a/src/images/rotation_images/20APR/random/left000.jpg b/src/images/rotation_images/20APR/random/left000.jpg new file mode 100644 index 0000000..a0c9b6a Binary files /dev/null and b/src/images/rotation_images/20APR/random/left000.jpg differ diff --git a/src/images/rotation_images/20APR/random/left001.jpg b/src/images/rotation_images/20APR/random/left001.jpg new file mode 100644 index 0000000..f9b1163 Binary files /dev/null and b/src/images/rotation_images/20APR/random/left001.jpg differ diff --git a/src/images/rotation_images/20APR/random/left002.jpg b/src/images/rotation_images/20APR/random/left002.jpg new file mode 100644 index 0000000..b6c9a85 Binary files /dev/null and b/src/images/rotation_images/20APR/random/left002.jpg differ diff --git a/src/images/rotation_images/20APR/random/right000.jpg b/src/images/rotation_images/20APR/random/right000.jpg new file mode 100644 index 0000000..d7282a3 Binary files /dev/null and b/src/images/rotation_images/20APR/random/right000.jpg differ diff --git a/src/images/rotation_images/20APR/random/right001.jpg b/src/images/rotation_images/20APR/random/right001.jpg new file mode 100644 index 0000000..4e15d24 Binary files /dev/null and b/src/images/rotation_images/20APR/random/right001.jpg differ diff --git a/src/images/rotation_images/20APR/random/right002.jpg b/src/images/rotation_images/20APR/random/right002.jpg new file mode 100644 index 0000000..abb5d6c Binary files /dev/null and b/src/images/rotation_images/20APR/random/right002.jpg differ diff --git a/src/images/rotation_images/20APR/right001.jpg b/src/images/rotation_images/20APR/right001.jpg new file mode 100644 index 0000000..d6c4574 Binary files /dev/null and b/src/images/rotation_images/20APR/right001.jpg differ diff --git a/src/images/rotation_images/20APR/right002.jpg b/src/images/rotation_images/20APR/right002.jpg new file mode 100644 index 0000000..22989eb Binary files /dev/null and b/src/images/rotation_images/20APR/right002.jpg differ diff --git a/src/images/rotation_images/20APR/right003.jpg b/src/images/rotation_images/20APR/right003.jpg new file mode 100644 index 0000000..af1766f Binary files /dev/null and b/src/images/rotation_images/20APR/right003.jpg differ diff --git a/src/images/rotation_images/20APR/right004.jpg b/src/images/rotation_images/20APR/right004.jpg new file mode 100644 index 0000000..fbc6155 Binary files /dev/null and b/src/images/rotation_images/20APR/right004.jpg differ diff --git a/src/images/rotation_images/20APR/right005.jpg b/src/images/rotation_images/20APR/right005.jpg new file mode 100644 index 0000000..c15a284 Binary files /dev/null and b/src/images/rotation_images/20APR/right005.jpg differ diff --git a/src/images/rotation_images/20APR/right006.jpg b/src/images/rotation_images/20APR/right006.jpg new file mode 100644 index 0000000..db83723 Binary files /dev/null and b/src/images/rotation_images/20APR/right006.jpg differ diff --git a/src/images/rotation_images/20APR/right007.jpg b/src/images/rotation_images/20APR/right007.jpg new file mode 100644 index 0000000..a352619 Binary files /dev/null and b/src/images/rotation_images/20APR/right007.jpg differ diff --git a/src/images/rotation_images/20APR/right008.jpg b/src/images/rotation_images/20APR/right008.jpg new file mode 100644 index 0000000..5ff9824 Binary files /dev/null and b/src/images/rotation_images/20APR/right008.jpg differ diff --git a/src/images/templates/19APR/left001.jpg b/src/images/templates/19APR/left001.jpg new file mode 100644 index 0000000..390e3b1 Binary files /dev/null and b/src/images/templates/19APR/left001.jpg differ diff --git a/src/images/templates/19APR/left002.jpg b/src/images/templates/19APR/left002.jpg new file mode 100644 index 0000000..848f2b5 Binary files /dev/null and b/src/images/templates/19APR/left002.jpg differ diff --git a/src/images/templates/19APR/left003.jpg b/src/images/templates/19APR/left003.jpg new file mode 100644 index 0000000..f58bba9 Binary files /dev/null and b/src/images/templates/19APR/left003.jpg differ diff --git a/src/images/templates/19APR/left004.jpg b/src/images/templates/19APR/left004.jpg new file mode 100644 index 0000000..2f23a74 Binary files /dev/null and b/src/images/templates/19APR/left004.jpg differ diff --git a/src/images/templates/19APR/left005.jpg b/src/images/templates/19APR/left005.jpg new file mode 100644 index 0000000..c545931 Binary files /dev/null and b/src/images/templates/19APR/left005.jpg differ diff --git a/src/images/templates/19APR/left006.jpg b/src/images/templates/19APR/left006.jpg new file mode 100644 index 0000000..207d4cf Binary files /dev/null and b/src/images/templates/19APR/left006.jpg differ diff --git a/src/images/templates/19APR/left007.jpg b/src/images/templates/19APR/left007.jpg new file mode 100644 index 0000000..beae29f Binary files /dev/null and b/src/images/templates/19APR/left007.jpg differ diff --git a/src/images/templates/19APR/left008.jpg b/src/images/templates/19APR/left008.jpg new file mode 100644 index 0000000..7bd5d47 Binary files /dev/null and b/src/images/templates/19APR/left008.jpg differ diff --git a/src/images/templates/19APR/right001.jpg b/src/images/templates/19APR/right001.jpg new file mode 100644 index 0000000..44d3cce Binary files /dev/null and b/src/images/templates/19APR/right001.jpg differ diff --git a/src/images/templates/19APR/right002.jpg b/src/images/templates/19APR/right002.jpg new file mode 100644 index 0000000..bb0369f Binary files /dev/null and b/src/images/templates/19APR/right002.jpg differ diff --git a/src/images/templates/19APR/right003.jpg b/src/images/templates/19APR/right003.jpg new file mode 100644 index 0000000..5e8e5de Binary files /dev/null and b/src/images/templates/19APR/right003.jpg differ diff --git a/src/images/templates/19APR/right004.jpg b/src/images/templates/19APR/right004.jpg new file mode 100644 index 0000000..e8f1201 Binary files /dev/null and b/src/images/templates/19APR/right004.jpg differ diff --git a/src/images/templates/19APR/right005.jpg b/src/images/templates/19APR/right005.jpg new file mode 100644 index 0000000..a1b1f0d Binary files /dev/null and b/src/images/templates/19APR/right005.jpg differ diff --git a/src/images/templates/19APR/right006.jpg b/src/images/templates/19APR/right006.jpg new file mode 100644 index 0000000..e2e278b Binary files /dev/null and b/src/images/templates/19APR/right006.jpg differ diff --git a/src/images/templates/19APR/right007.jpg b/src/images/templates/19APR/right007.jpg new file mode 100644 index 0000000..50b68ec Binary files /dev/null and b/src/images/templates/19APR/right007.jpg differ diff --git a/src/images/templates/19APR/right008.jpg b/src/images/templates/19APR/right008.jpg new file mode 100644 index 0000000..ef71909 Binary files /dev/null and b/src/images/templates/19APR/right008.jpg differ diff --git a/src/images/templates/20APR/left001.jpg b/src/images/templates/20APR/left001.jpg new file mode 100644 index 0000000..a79f882 Binary files /dev/null and b/src/images/templates/20APR/left001.jpg differ diff --git a/src/images/templates/20APR/left002.jpg b/src/images/templates/20APR/left002.jpg new file mode 100644 index 0000000..1d4a49f Binary files /dev/null and b/src/images/templates/20APR/left002.jpg differ diff --git a/src/images/templates/20APR/left003.jpg b/src/images/templates/20APR/left003.jpg new file mode 100644 index 0000000..e5ba573 Binary files /dev/null and b/src/images/templates/20APR/left003.jpg differ diff --git a/src/images/templates/20APR/left004.jpg b/src/images/templates/20APR/left004.jpg new file mode 100644 index 0000000..9953c1e Binary files /dev/null and b/src/images/templates/20APR/left004.jpg differ diff --git a/src/images/templates/20APR/left005.jpg b/src/images/templates/20APR/left005.jpg new file mode 100644 index 0000000..015555f Binary files /dev/null and b/src/images/templates/20APR/left005.jpg differ diff --git a/src/images/templates/20APR/left006.jpg b/src/images/templates/20APR/left006.jpg new file mode 100644 index 0000000..741a20c Binary files /dev/null and b/src/images/templates/20APR/left006.jpg differ diff --git a/src/images/templates/20APR/left007.jpg b/src/images/templates/20APR/left007.jpg new file mode 100644 index 0000000..9dab9f2 Binary files /dev/null and b/src/images/templates/20APR/left007.jpg differ diff --git a/src/images/templates/20APR/left008.jpg b/src/images/templates/20APR/left008.jpg new file mode 100644 index 0000000..0650b41 Binary files /dev/null and b/src/images/templates/20APR/left008.jpg differ diff --git a/src/images/templates/20APR/leftGoal.jpg b/src/images/templates/20APR/leftGoal.jpg new file mode 100644 index 0000000..6718131 Binary files /dev/null and b/src/images/templates/20APR/leftGoal.jpg differ diff --git a/src/images/templates/20APR/right001.jpg b/src/images/templates/20APR/right001.jpg new file mode 100644 index 0000000..d6c4574 Binary files /dev/null and b/src/images/templates/20APR/right001.jpg differ diff --git a/src/images/templates/20APR/right002.jpg b/src/images/templates/20APR/right002.jpg new file mode 100644 index 0000000..22989eb Binary files /dev/null and b/src/images/templates/20APR/right002.jpg differ diff --git a/src/images/templates/20APR/right003.jpg b/src/images/templates/20APR/right003.jpg new file mode 100644 index 0000000..af1766f Binary files /dev/null and b/src/images/templates/20APR/right003.jpg differ diff --git a/src/images/templates/20APR/right004.jpg b/src/images/templates/20APR/right004.jpg new file mode 100644 index 0000000..fbc6155 Binary files /dev/null and b/src/images/templates/20APR/right004.jpg differ diff --git a/src/images/templates/20APR/right005.jpg b/src/images/templates/20APR/right005.jpg new file mode 100644 index 0000000..c15a284 Binary files /dev/null and b/src/images/templates/20APR/right005.jpg differ diff --git a/src/images/templates/20APR/right006.jpg b/src/images/templates/20APR/right006.jpg new file mode 100644 index 0000000..db83723 Binary files /dev/null and b/src/images/templates/20APR/right006.jpg differ diff --git a/src/images/templates/20APR/right007.jpg b/src/images/templates/20APR/right007.jpg new file mode 100644 index 0000000..a352619 Binary files /dev/null and b/src/images/templates/20APR/right007.jpg differ diff --git a/src/images/templates/20APR/right008.jpg b/src/images/templates/20APR/right008.jpg new file mode 100644 index 0000000..5ff9824 Binary files /dev/null and b/src/images/templates/20APR/right008.jpg differ diff --git a/src/images/templates/20APR/rightGoal.jpg b/src/images/templates/20APR/rightGoal.jpg new file mode 100644 index 0000000..213fe07 Binary files /dev/null and b/src/images/templates/20APR/rightGoal.jpg differ diff --git a/src/images/templates/left001.jpg b/src/images/templates/left001.jpg new file mode 100644 index 0000000..2134459 Binary files /dev/null and b/src/images/templates/left001.jpg differ diff --git a/src/images/templates/left002.jpg b/src/images/templates/left002.jpg new file mode 100644 index 0000000..c5913f2 Binary files /dev/null and b/src/images/templates/left002.jpg differ diff --git a/src/images/templates/left003.jpg b/src/images/templates/left003.jpg new file mode 100644 index 0000000..949b254 Binary files /dev/null and b/src/images/templates/left003.jpg differ diff --git a/src/images/templates/left004.jpg b/src/images/templates/left004.jpg new file mode 100644 index 0000000..08a5401 Binary files /dev/null and b/src/images/templates/left004.jpg differ diff --git a/src/images/templates/left005.jpg b/src/images/templates/left005.jpg new file mode 100644 index 0000000..a30de80 Binary files /dev/null and b/src/images/templates/left005.jpg differ diff --git a/src/images/templates/left006.jpg b/src/images/templates/left006.jpg new file mode 100644 index 0000000..49a4159 Binary files /dev/null and b/src/images/templates/left006.jpg differ diff --git a/src/images/templates/left007.jpg b/src/images/templates/left007.jpg new file mode 100644 index 0000000..7760e0b Binary files /dev/null and b/src/images/templates/left007.jpg differ diff --git a/src/images/templates/left008.jpg b/src/images/templates/left008.jpg new file mode 100644 index 0000000..d29e9a8 Binary files /dev/null and b/src/images/templates/left008.jpg differ diff --git a/src/images/templates/left010.jpg b/src/images/templates/left010.jpg new file mode 100644 index 0000000..ce27d50 Binary files /dev/null and b/src/images/templates/left010.jpg differ diff --git a/src/images/templates/left011.jpg b/src/images/templates/left011.jpg new file mode 100644 index 0000000..6101f97 Binary files /dev/null and b/src/images/templates/left011.jpg differ diff --git a/src/images/templates/left013.jpg b/src/images/templates/left013.jpg new file mode 100644 index 0000000..749b6a5 Binary files /dev/null and b/src/images/templates/left013.jpg differ diff --git a/src/images/templates/left014.jpg b/src/images/templates/left014.jpg new file mode 100644 index 0000000..f7de3d5 Binary files /dev/null and b/src/images/templates/left014.jpg differ diff --git a/src/images/templates/left029.jpg b/src/images/templates/left029.jpg new file mode 100644 index 0000000..064692a Binary files /dev/null and b/src/images/templates/left029.jpg differ diff --git a/src/images/templates/left030.jpg b/src/images/templates/left030.jpg new file mode 100644 index 0000000..8891bb6 Binary files /dev/null and b/src/images/templates/left030.jpg differ diff --git a/src/images/templates/left031.jpg b/src/images/templates/left031.jpg new file mode 100644 index 0000000..fa2c787 Binary files /dev/null and b/src/images/templates/left031.jpg differ diff --git a/src/images/templates/left032.jpg b/src/images/templates/left032.jpg new file mode 100644 index 0000000..1958778 Binary files /dev/null and b/src/images/templates/left032.jpg differ diff --git a/src/images/templates/left033.jpg b/src/images/templates/left033.jpg new file mode 100644 index 0000000..b7c8f60 Binary files /dev/null and b/src/images/templates/left033.jpg differ diff --git a/src/images/templates/left034.jpg b/src/images/templates/left034.jpg new file mode 100644 index 0000000..60618ff Binary files /dev/null and b/src/images/templates/left034.jpg differ diff --git a/src/images/templates/left035.jpg b/src/images/templates/left035.jpg new file mode 100644 index 0000000..a81ebba Binary files /dev/null and b/src/images/templates/left035.jpg differ diff --git a/src/images/templates/right001.jpg b/src/images/templates/right001.jpg new file mode 100644 index 0000000..f2ca6c1 Binary files /dev/null and b/src/images/templates/right001.jpg differ diff --git a/src/images/templates/right002.jpg b/src/images/templates/right002.jpg new file mode 100644 index 0000000..862c1c8 Binary files /dev/null and b/src/images/templates/right002.jpg differ diff --git a/src/images/templates/right003.jpg b/src/images/templates/right003.jpg new file mode 100644 index 0000000..4e6fc57 Binary files /dev/null and b/src/images/templates/right003.jpg differ diff --git a/src/images/templates/right004.jpg b/src/images/templates/right004.jpg new file mode 100644 index 0000000..5ea6c72 Binary files /dev/null and b/src/images/templates/right004.jpg differ diff --git a/src/images/templates/right005.jpg b/src/images/templates/right005.jpg new file mode 100644 index 0000000..edd9308 Binary files /dev/null and b/src/images/templates/right005.jpg differ diff --git a/src/images/templates/right006.jpg b/src/images/templates/right006.jpg new file mode 100644 index 0000000..a217ec3 Binary files /dev/null and b/src/images/templates/right006.jpg differ diff --git a/src/images/templates/right007.jpg b/src/images/templates/right007.jpg new file mode 100644 index 0000000..6367a17 Binary files /dev/null and b/src/images/templates/right007.jpg differ diff --git a/src/images/templates/right008.jpg b/src/images/templates/right008.jpg new file mode 100644 index 0000000..a00c332 Binary files /dev/null and b/src/images/templates/right008.jpg differ diff --git a/src/images/templates/right009.jpg b/src/images/templates/right009.jpg new file mode 100644 index 0000000..13018cf Binary files /dev/null and b/src/images/templates/right009.jpg differ diff --git a/src/images/templates/right010.jpg b/src/images/templates/right010.jpg new file mode 100644 index 0000000..f3d058b Binary files /dev/null and b/src/images/templates/right010.jpg differ diff --git a/src/images/templates/right011.jpg b/src/images/templates/right011.jpg new file mode 100644 index 0000000..aafb472 Binary files /dev/null and b/src/images/templates/right011.jpg differ diff --git a/src/images/templates/right013.jpg b/src/images/templates/right013.jpg new file mode 100644 index 0000000..0fb1de6 Binary files /dev/null and b/src/images/templates/right013.jpg differ diff --git a/src/images/templates/right014.jpg b/src/images/templates/right014.jpg new file mode 100644 index 0000000..9360978 Binary files /dev/null and b/src/images/templates/right014.jpg differ diff --git a/src/images/templates/right029.jpg b/src/images/templates/right029.jpg new file mode 100644 index 0000000..08de483 Binary files /dev/null and b/src/images/templates/right029.jpg differ diff --git a/src/images/templates/right030.jpg b/src/images/templates/right030.jpg new file mode 100644 index 0000000..64a6a6e Binary files /dev/null and b/src/images/templates/right030.jpg differ diff --git a/src/images/templates/right031.jpg b/src/images/templates/right031.jpg new file mode 100644 index 0000000..87e85cb Binary files /dev/null and b/src/images/templates/right031.jpg differ diff --git a/src/images/templates/right032.jpg b/src/images/templates/right032.jpg new file mode 100644 index 0000000..67fd347 Binary files /dev/null and b/src/images/templates/right032.jpg differ diff --git a/src/images/templates/right033.jpg b/src/images/templates/right033.jpg new file mode 100644 index 0000000..a5f984b Binary files /dev/null and b/src/images/templates/right033.jpg differ diff --git a/src/images/templates/right034.jpg b/src/images/templates/right034.jpg new file mode 100644 index 0000000..73de174 Binary files /dev/null and b/src/images/templates/right034.jpg differ diff --git a/src/images/templates/right035.jpg b/src/images/templates/right035.jpg new file mode 100644 index 0000000..16e369f Binary files /dev/null and b/src/images/templates/right035.jpg differ diff --git a/src/images/test_images/.DS_Store b/src/images/test_images/.DS_Store new file mode 100644 index 0000000..5008ddf Binary files /dev/null and b/src/images/test_images/.DS_Store differ diff --git a/src/images/test_images/21APR/left000.jpg b/src/images/test_images/21APR/left000.jpg new file mode 100644 index 0000000..5845bb0 Binary files /dev/null and b/src/images/test_images/21APR/left000.jpg differ diff --git a/src/images/test_images/21APR/left001.jpg b/src/images/test_images/21APR/left001.jpg new file mode 100644 index 0000000..3cb2a53 Binary files /dev/null and b/src/images/test_images/21APR/left001.jpg differ diff --git a/src/images/test_images/21APR/left002.jpg b/src/images/test_images/21APR/left002.jpg new file mode 100644 index 0000000..4739cdc Binary files /dev/null and b/src/images/test_images/21APR/left002.jpg differ diff --git a/src/images/test_images/21APR/left003.jpg b/src/images/test_images/21APR/left003.jpg new file mode 100644 index 0000000..17a9b84 Binary files /dev/null and b/src/images/test_images/21APR/left003.jpg differ diff --git a/src/images/test_images/21APR/left004.jpg b/src/images/test_images/21APR/left004.jpg new file mode 100644 index 0000000..ae96271 Binary files /dev/null and b/src/images/test_images/21APR/left004.jpg differ diff --git a/src/images/test_images/21APR/left005.jpg b/src/images/test_images/21APR/left005.jpg new file mode 100644 index 0000000..cca5ba5 Binary files /dev/null and b/src/images/test_images/21APR/left005.jpg differ diff --git a/src/images/test_images/21APR/right000.jpg b/src/images/test_images/21APR/right000.jpg new file mode 100644 index 0000000..f83d608 Binary files /dev/null and b/src/images/test_images/21APR/right000.jpg differ diff --git a/src/images/test_images/21APR/right001.jpg b/src/images/test_images/21APR/right001.jpg new file mode 100644 index 0000000..6837a03 Binary files /dev/null and b/src/images/test_images/21APR/right001.jpg differ diff --git a/src/images/test_images/21APR/right002.jpg b/src/images/test_images/21APR/right002.jpg new file mode 100644 index 0000000..21945ec Binary files /dev/null and b/src/images/test_images/21APR/right002.jpg differ diff --git a/src/images/test_images/21APR/right003.jpg b/src/images/test_images/21APR/right003.jpg new file mode 100644 index 0000000..94349d6 Binary files /dev/null and b/src/images/test_images/21APR/right003.jpg differ diff --git a/src/images/test_images/21APR/right004.jpg b/src/images/test_images/21APR/right004.jpg new file mode 100644 index 0000000..bd30e22 Binary files /dev/null and b/src/images/test_images/21APR/right004.jpg differ diff --git a/src/images/test_images/21APR/right005.jpg b/src/images/test_images/21APR/right005.jpg new file mode 100644 index 0000000..40e617a Binary files /dev/null and b/src/images/test_images/21APR/right005.jpg differ diff --git a/src/images/test_images/left034.jpg b/src/images/test_images/left034.jpg new file mode 100644 index 0000000..a9f67c9 Binary files /dev/null and b/src/images/test_images/left034.jpg differ diff --git a/src/images/test_images/left036.jpg b/src/images/test_images/left036.jpg new file mode 100644 index 0000000..e5eba35 Binary files /dev/null and b/src/images/test_images/left036.jpg differ diff --git a/src/images/test_images/left037.jpg b/src/images/test_images/left037.jpg new file mode 100644 index 0000000..884817a Binary files /dev/null and b/src/images/test_images/left037.jpg differ diff --git a/src/images/test_images/left038.jpg b/src/images/test_images/left038.jpg new file mode 100644 index 0000000..d851dcc Binary files /dev/null and b/src/images/test_images/left038.jpg differ diff --git a/src/images/test_images/left039.jpg b/src/images/test_images/left039.jpg new file mode 100644 index 0000000..3ed5ccc Binary files /dev/null and b/src/images/test_images/left039.jpg differ diff --git a/src/images/test_images/left040.jpg b/src/images/test_images/left040.jpg new file mode 100644 index 0000000..2a4d6b3 Binary files /dev/null and b/src/images/test_images/left040.jpg differ diff --git a/src/images/test_images/right034.jpg b/src/images/test_images/right034.jpg new file mode 100644 index 0000000..9a949e3 Binary files /dev/null and b/src/images/test_images/right034.jpg differ diff --git a/src/images/test_images/right036.jpg b/src/images/test_images/right036.jpg new file mode 100644 index 0000000..93e095e Binary files /dev/null and b/src/images/test_images/right036.jpg differ diff --git a/src/images/test_images/right037.jpg b/src/images/test_images/right037.jpg new file mode 100644 index 0000000..b7490ac Binary files /dev/null and b/src/images/test_images/right037.jpg differ diff --git a/src/images/test_images/right038.jpg b/src/images/test_images/right038.jpg new file mode 100644 index 0000000..c7405dc Binary files /dev/null and b/src/images/test_images/right038.jpg differ diff --git a/src/images/test_images/right039.jpg b/src/images/test_images/right039.jpg new file mode 100644 index 0000000..67a0981 Binary files /dev/null and b/src/images/test_images/right039.jpg differ diff --git a/src/images/test_images/right040.jpg b/src/images/test_images/right040.jpg new file mode 100644 index 0000000..12d66be Binary files /dev/null and b/src/images/test_images/right040.jpg differ diff --git a/src/local_controller.cpp b/src/local_controller.cpp new file mode 100644 index 0000000..7a4f2a0 --- /dev/null +++ b/src/local_controller.cpp @@ -0,0 +1,176 @@ +/* + * Local controller to follow a trajectory + * + * By: Eric Danziger + * March 1, 2015 + */ + + +#include "trajectory_controller/local_controller.h" + + +localController::localController() +{ + currentPose.position.x = 0; + currentPose.position.y = 0; + +} + +localController::~localController() +{ +} + +void localController::publishMessage(ros::Publisher *pub_message) +{ + //Go through the path, find all poses within 1m of currentPose + nav_msgs::Path pathWindow; + double timeStep = .2; + bool alreadyLast = false; + std::vector dVel,dYaw; + double closestDist = 10000; + int closestIndex; + geometry_msgs::Twist atDes, toDes; + geometry_msgs::Twist msg; + if(currentPath.poses.size() < 1) + { + pub_message->publish(msg); + return; + } + for(int i=0;i 100) closestIndex = 1; + if (closestIndex == pathWindow.poses.size() -1 && !alreadyLast) alreadyLast = true; + else if(closestIndex == pathWindow.poses.size() -1) closestIndex = pathWindow.poses.size(); + + ROS_INFO("Closest index %d",closestIndex); + ROS_INFO("size %d :: %d", dVel.size(), dYaw.size()); + //figure out the desired twist at the desired point + double xVel = (dVel[closestIndex-1] + dVel[closestIndex]) /2; + double aVel = (dYaw[closestIndex-1] - dYaw[closestIndex])/timeStep; + ROS_INFO("desired linear Vel: %f",xVel); + ROS_INFO("desired angular Vel: %f",aVel); + //figure out the twist to get to the desired point + //xdiff + double xdiff = pathWindow.poses[closestIndex].pose.position.x - currentPose.position.x; + //ydiff + double ydiff = pathWindow.poses[closestIndex].pose.position.y - currentPose.position.y; + //desired velocity along the line + long double desVel = xdiff/fabs(xdiff) * sqrt(xdiff*xdiff + ydiff*ydiff);///timeStep ; + ROS_INFO("xdiff %f", xdiff); + ROS_INFO("ydiff %f", ydiff); + ROS_INFO("sqrt %f", sqrt(xdiff*xdiff + ydiff*ydiff)); + ROS_INFO("xdiff %f", xdiff/fabs(xdiff)); + //ROS_INFO("path x : %f", pathWindow.poses[closestIndex].pose.position.x); + //ROS_INFO("currentPose x: %f", currentPose.position.x); + //ROS_INFO("path y : %f", pathWindow.poses[closestIndex].pose.position.y); + //ROS_INFO("currentPose y: %f", currentPose.position.y); + + + ROS_INFO("desiredVelocity %f", desVel); + //current yaw + double roll,pitch,yaw; + //currentPose.orientation.getRPY(roll,pitch,yaw); + tf::Quaternion q = tf::Quaternion(currentPose.orientation.x,currentPose.orientation.y,currentPose.orientation.z,currentPose.orientation.w); + tf::Matrix3x3 m(q); + + m.getRPY(roll,pitch,yaw); + ROS_INFO("current yaw %f",yaw); + //desired yaw to match the line + double desAng = (atan2(ydiff,xdiff)-yaw); + //average them to create the twist to publish + msg.linear.x = (desVel + xVel) / 2; + if(msg.linear.x > 0.1){ + msg.linear.x = 0.1; + } else if(msg.linear.x < -0.1){ + msg.linear.x = -0.1; + } + msg.angular.z = 0.1*((desAng + aVel) / 2); + + msg.angular.z = 0.0; + pub_message->publish(msg); + +} + +void localController::pathMessageCallback(const nav_msgs::Path::ConstPtr &msg) +{ + //get a path + + //update current path + currentPath = *msg; + pathCounter = 0; + + ROS_INFO("Got a New Path\n"); +} + +void localController::odomUpdateCallback(const nav_msgs::Odometry::ConstPtr &msg) +{ + //update position in worldFrame + currentVel = msg->twist.twist; + currentPose = msg->pose.pose; + ROS_INFO("Got a New Odom\n"); +} + + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "locol_controller"); + ros::NodeHandle nh; + + localController *local_controller = new localController(); + ros::Publisher pub = nh.advertise("/cmd_vel",10); + ros::Subscriber odom_sub = nh.subscribe("/odom",1000,&localController::odomUpdateCallback,local_controller); + ros::Subscriber path_sub = nh.subscribe("/plan",1000,&localController::pathMessageCallback,local_controller); + ros::Rate r(10); + + while(nh.ok()) + { + local_controller->publishMessage(&pub); + r.sleep(); + ros::spinOnce(); + } + + return 0; +} diff --git a/src/matlab2opencv.m b/src/matlab2opencv.m new file mode 100644 index 0000000..5ebf180 --- /dev/null +++ b/src/matlab2opencv.m @@ -0,0 +1,41 @@ +function matlab2opencv( variable, fileName, flag) + +[rows cols] = size(variable); + +% Beware of Matlab's linear indexing +variable = variable'; + +% Write mode as default +if ( ~exist('flag','var') ) + flag = 'w'; +end + +if ( ~exist(fileName,'file') || flag == 'w' ) + % New file or write mode specified + file = fopen( fileName, 'w'); + fprintf( file, '%%YAML:1.0\n'); +else + % Append mode + file = fopen( fileName, 'a'); +end + +% Write variable header +fprintf( file, ' %s: !!opencv-matrix\n', inputname(1)); +fprintf( file, ' rows: %d\n', rows); +fprintf( file, ' cols: %d\n', cols); +fprintf( file, ' dt: f\n'); +fprintf( file, ' data: [ '); + +% Write variable data +for i=1:rows*cols + fprintf( file, '%.6f', variable(i)); + if (i == rows*cols), break, end + fprintf( file, ', '); + if mod(i+1,4) == 0 + fprintf( file, '\n '); + end +end + +fprintf( file, ']\n'); + +fclose(file); \ No newline at end of file diff --git a/src/odd_calibration.m b/src/odd_calibration.m new file mode 100644 index 0000000..d99a6bc --- /dev/null +++ b/src/odd_calibration.m @@ -0,0 +1,26 @@ + image_dir = '/home/ed/Dropbox/GitHub/catkin_ws/src/perception_system/src/images/calibration_images/20APR'; +leftImages = imageSet(fullfile(image_dir,'left')); +rightImages = imageSet(fullfile(image_dir,'right')); +images1 = leftImages.ImageLocation; +images2 = rightImages.ImageLocation; +[imagePoints,boardSize,pairsUsed] = detectCheckerboardPoints(images1,images2); +squareSize = 22.5; +worldPoints = generateCheckerboardPoints(boardSize,squareSize); +stereoParams = estimateCameraParameters(imagePoints,worldPoints); +save('/home/ed/Dropbox/GitHub/catkin_ws/src/perception_system/src/images/calibration_images/20APR/stereoParams.mat','stereoParams'); +save('/home/ed/Dropbox/GitHub/catkin_ws/src/perception_system/src/images/calibration_images/20APR/calibParams.mat'); +K1 = stereoParams.CameraParameters1.IntrinsicMatrix; +K2 = stereoParams.CameraParameters2.IntrinsicMatrix; +M1 = [eye(3) zeros(3,1)]; +R2 = stereoParams.RotationOfCamera2; +t2 = stereoParams.TranslationOfCamera2; +M2 = [R2 t2']; +P1 = K1*M1; +P2 = K2*M2; + + +matlab2opencv(P1,'20APRprojmats.yml'); +matlab2opencv(P2,'20APRprojmats.yml','a'); + + + diff --git a/src/pc b/src/pc new file mode 100755 index 0000000..d52e350 Binary files /dev/null and b/src/pc differ diff --git a/src/perception_capture.cpp b/src/perception_capture.cpp new file mode 100644 index 0000000..38d766b --- /dev/null +++ b/src/perception_capture.cpp @@ -0,0 +1,130 @@ +#include +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/core/core.hpp" +#include +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include + + +using namespace cv; +using namespace std; + +int main(int argc, char** argv) +{ + + string folders; + if (argc !=2) + { + folders = ""; + } + else + { + folders = argv[1]; + } + + char *folder = new char[folders.length()+1]; + strcpy(folder,folders.c_str()); + + VideoCapture capture1(1); // open the video file for reading + VideoCapture capture2(2); + //VideoCapture capture1(1); // open the video file for reading + //capture0.set(CV_CAP_PROP_FRAME_WIDTH,640); + //capture0.set(CV_CAP_PROP_FRAME_HEIGHT,480); + capture1.set(CV_CAP_PROP_FRAME_WIDTH,1280); + capture1.set(CV_CAP_PROP_FRAME_HEIGHT,720); + capture2.set(CV_CAP_PROP_FRAME_WIDTH,1280); + capture2.set(CV_CAP_PROP_FRAME_HEIGHT,720); + + int key = 0; + int n; + Mat frame1,frame2; + vector leftImages, rightImages; + //while(key != 'q') + for(int i = 0; i < 100000; i++) + { + + if(!capture1.isOpened()){ + cout << "Failed to connect to the camera 1." << endl; + } + if(!capture2.isOpened()){ + cout << "Failed to connect to the camera 2." << endl; + } + + capture1 >> frame1; + //waitKey(10); + capture2 >> frame2; + //waitKey(10); + if(frame1.empty()){ + cout << "Failed to capture an image on camera 1" << endl; + return -1; + } + + if(frame2.empty()){ + cout << "Failed to capture an image on camera 2" << endl; + return -1; + } + + leftImages.push_back(frame1); + + rightImages.push_back(frame2); + + imshow("leftImage",frame1); + imshow("rightImage",frame2); + + char buffer[65]; + int speed = 300; + if(i%speed == 0) + { + n = sprintf(buffer,"images/%s/left%03d.jpg",folder,i/speed); + printf("this is the folder: %s\n",buffer); + //waitKey(0); + imwrite(buffer,frame1); + n = sprintf(buffer,"images/%s/right%03d.jpg",folder,i/speed); + imwrite(buffer,frame2); + Mat lempty = frame1; + GaussianBlur(lempty,lempty,Size(97,97),11,11); + imshow("leftImage",lempty); + imshow("rightImage",lempty); + waitKey(900); + } + + waitKey(10); + } + + //start to compare the images + + + //-- Detect the keypoints using SURF Detector + // int minHessian = 400; + + // SurfFeatureDetector detector( minHessian ); + + // std::vector keypoints1, keypoints2, keypointsInBox1, keypointsGM1, keypointsGM2; + + // detector.detect( frame1, keypoints1 ); + // detector.detect( frame2, keypoints2 ); + + // Mat imgKeypoints1; Mat imgKeypoints2; + + //-- Detect keypoints that are within the box + + // for (int i = 0; i < keypoints1.size(); i++) + // { + // if (keypoints1[i].pt.x < lowerRight.x && keypoints1[i].pt.x > upperLeft.x && + // keypoints1[i].pt.y < lowerRight.y && keypoints1[i].pt.y > upperLeft.y) + // { + // keypointsInBox1.push_back(keypoints1[i]); + // } + + // } + // //-- Match points from within the box to the next frame + // matchPoints(&keypointsInBox1,&keypoints2,image1,image2,&keypointsGM1,&keypointsGM2,2.1); + + + //key = waitKey(); + // cout << key << endl; + + + return 0; +} diff --git a/src/perception_capture.cpp~ b/src/perception_capture.cpp~ new file mode 100644 index 0000000..2115579 --- /dev/null +++ b/src/perception_capture.cpp~ @@ -0,0 +1,9 @@ +#include +#include +#include + +int main() +{ + + +} diff --git a/src/perception_tracker.cpp b/src/perception_tracker.cpp new file mode 100644 index 0000000..bad1d36 --- /dev/null +++ b/src/perception_tracker.cpp @@ -0,0 +1,186 @@ +/* + * Peception tracker for Robot + * + * By: Eric Danziger + * March 1, 2015 + */ + + +#include "perception_system/perception_tracker.h" +#include "perception_system/bot.h" + + +perceptionTracker::perceptionTracker() +{ + currentPose.position.x = 0; + currentPose.position.y = 0; + +} + +perceptionTracker::~perceptionTracker() +{ +} + +void perceptionTracker::publishMessage(ros::Publisher *pub_message) +{ + //Go through the path, find all poses within 1m of currentPose + nav_msgs::Path pathWindow; + double timeStep = .2; + bool alreadyLast = false; + std::vector dVel,dYaw; + double closestDist = 10000; + int closestIndex; + geometry_msgs::Twist atDes, toDes; + geometry_msgs::Twist msg; + + pub_message->publish(msg); + +} + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "perception_tracker"); + ros::NodeHandle nh; + int showI; + if(argc < 2) + showI = 0; + else + showI = atoi(argv[1]); + Bot *oddBot = new Bot(showI); + + float xRobot,yRobot,tRobot; + float xGoal,yGoal,tGoal; + float posError; + + perceptionTracker *perc_track= new perceptionTracker(); + + ros::Publisher rPub = nh.advertise("/robot_perceived_pose",10); + ros::Publisher gPub = nh.advertise("/goal_perceived_pose",1,true); + + ros::Publisher relPub = nh.advertise("/plan",10); + + perc_track->goalPose.x = 1.9; + perc_track->goalPose.y = 2.5; + gPub.publish(perc_track->goalPose); + + ros::Rate r(100); + + cv::VideoCapture capture1(1); // open the video file for reading + cv::VideoCapture capture2(2); + //VideoCapture capture1(1); // open the video file for reading + //capture0.set(CV_CAP_PROP_FRAME_WIDTH,640); + //capture0.set(CV_CAP_PROP_FRAME_HEIGHT,480); + capture1.set(CV_CAP_PROP_FRAME_WIDTH,1280); + capture1.set(CV_CAP_PROP_FRAME_HEIGHT,720); + capture2.set(CV_CAP_PROP_FRAME_WIDTH,1280); + capture2.set(CV_CAP_PROP_FRAME_HEIGHT,720); + + + if(!capture1.isOpened()){ + std::cout << "Failed to connect to the camera 1." << std::endl; + } + if(!capture2.isOpened()){ + std::cout << "Failed to connect to the camera 2." << std::endl; + } + + + while(nh.ok()) + { + //Do the OPENCV stuff + + cv::Mat imgL,imgR; + capture1>>imgL; + capture2>>imgR; + + // while(!imgL.empty()) + for(int i = 0; i < 100; i++) + { + capture1>>imgL; + capture2>>imgR; + waitKey(1); + } + waitKey(1); + capture1>>imgL; + capture2>>imgR; + + if(imgL.empty()){ + std::cout << "Failed to capture an image on camera 1" << std::endl; + return -1; + } + if(imgR.empty()){ + std::cout << "Failed to capture an image on camera 2" << std::endl; + return -1; + } + // cv::imshow("Left Image - ", imgL); + // waitKey(1000); + oddBot->updateBoxPos(imgL, imgR,&xRobot,&yRobot,&tRobot); + perc_track->robotPose.x = xRobot; + perc_track->robotPose.y = yRobot; + perc_track->robotPose.theta = tRobot; + rPub.publish(perc_track->robotPose); + + if(argc > 2) + { + oddBot->findGoalPos(imgL, imgR,&xGoal,&yGoal,&tGoal); + perc_track->goalPose.x = xGoal; + perc_track->goalPose.y = yGoal; + perc_track->goalPose.theta = tGoal; + gPub.publish(perc_track->goalPose); + + } + + //-- Calculate In Robot Frame + float radius = sqrt( pow(perc_track->goalPose.x - perc_track->robotPose.x,2) + pow(perc_track->goalPose.y - perc_track->robotPose.y,2)); + //Is y or x on top? + float phi = atan2( (perc_track->goalPose.y - perc_track->robotPose.y) , (perc_track->goalPose.x - perc_track->robotPose.x) ); + if(phi < 0) + phi = 6.28 + phi; + //Is this order correct? + float phi2 = phi - (perc_track->robotPose.theta*3.14)/180.0; + + if (phi2 < 0) + phi2 = 2*3.14 - phi; + + printf("changed angles: %f %f\n",phi,phi2); + + printf("alternate phi: %f\n",atan2( (perc_track->goalPose.x - perc_track->robotPose.x) , (perc_track->goalPose.y - perc_track->robotPose.y) )); + + printf("radius: %f\n",radius); + + float ratio = tan(phi2); + + //float y2 = sqrt( pow(radius,2) / (1 + pow(ratio,2))); + + //float x2 = sqrt( pow(radius,2) - pow(y2,2) ); + + float y2 = radius * sin(phi2); + + float x2 = radius * cos(phi2); + + geometry_msgs::Pose2D relativePose; + relativePose.x = x2; + relativePose.y = y2; + + geometry_msgs::PoseStamped relPose; + relPose.pose.position.x = x2; + relPose.pose.position.y = y2; + + nav_msgs::Path relPath; + relPath.poses.push_back(relPose); + + relPub.publish(relPath); + + posError = sqrt(pow((perc_track->goalPose.x - perc_track->robotPose.x),2) + pow((perc_track->goalPose.y - perc_track->robotPose.y),2)); + printf("Current robot distance from goal: %f\n",posError); + if(posError < .1) + { + printf("\n#------- GOAL ACHIEVED!!!!! -----------#\n"); + return 0; + } + r.sleep(); + ros::spinOnce(); + } + + return 0; +} diff --git a/src/perception_tracker.cpp~ b/src/perception_tracker.cpp~ new file mode 100644 index 0000000..7a4f2a0 --- /dev/null +++ b/src/perception_tracker.cpp~ @@ -0,0 +1,176 @@ +/* + * Local controller to follow a trajectory + * + * By: Eric Danziger + * March 1, 2015 + */ + + +#include "trajectory_controller/local_controller.h" + + +localController::localController() +{ + currentPose.position.x = 0; + currentPose.position.y = 0; + +} + +localController::~localController() +{ +} + +void localController::publishMessage(ros::Publisher *pub_message) +{ + //Go through the path, find all poses within 1m of currentPose + nav_msgs::Path pathWindow; + double timeStep = .2; + bool alreadyLast = false; + std::vector dVel,dYaw; + double closestDist = 10000; + int closestIndex; + geometry_msgs::Twist atDes, toDes; + geometry_msgs::Twist msg; + if(currentPath.poses.size() < 1) + { + pub_message->publish(msg); + return; + } + for(int i=0;i 100) closestIndex = 1; + if (closestIndex == pathWindow.poses.size() -1 && !alreadyLast) alreadyLast = true; + else if(closestIndex == pathWindow.poses.size() -1) closestIndex = pathWindow.poses.size(); + + ROS_INFO("Closest index %d",closestIndex); + ROS_INFO("size %d :: %d", dVel.size(), dYaw.size()); + //figure out the desired twist at the desired point + double xVel = (dVel[closestIndex-1] + dVel[closestIndex]) /2; + double aVel = (dYaw[closestIndex-1] - dYaw[closestIndex])/timeStep; + ROS_INFO("desired linear Vel: %f",xVel); + ROS_INFO("desired angular Vel: %f",aVel); + //figure out the twist to get to the desired point + //xdiff + double xdiff = pathWindow.poses[closestIndex].pose.position.x - currentPose.position.x; + //ydiff + double ydiff = pathWindow.poses[closestIndex].pose.position.y - currentPose.position.y; + //desired velocity along the line + long double desVel = xdiff/fabs(xdiff) * sqrt(xdiff*xdiff + ydiff*ydiff);///timeStep ; + ROS_INFO("xdiff %f", xdiff); + ROS_INFO("ydiff %f", ydiff); + ROS_INFO("sqrt %f", sqrt(xdiff*xdiff + ydiff*ydiff)); + ROS_INFO("xdiff %f", xdiff/fabs(xdiff)); + //ROS_INFO("path x : %f", pathWindow.poses[closestIndex].pose.position.x); + //ROS_INFO("currentPose x: %f", currentPose.position.x); + //ROS_INFO("path y : %f", pathWindow.poses[closestIndex].pose.position.y); + //ROS_INFO("currentPose y: %f", currentPose.position.y); + + + ROS_INFO("desiredVelocity %f", desVel); + //current yaw + double roll,pitch,yaw; + //currentPose.orientation.getRPY(roll,pitch,yaw); + tf::Quaternion q = tf::Quaternion(currentPose.orientation.x,currentPose.orientation.y,currentPose.orientation.z,currentPose.orientation.w); + tf::Matrix3x3 m(q); + + m.getRPY(roll,pitch,yaw); + ROS_INFO("current yaw %f",yaw); + //desired yaw to match the line + double desAng = (atan2(ydiff,xdiff)-yaw); + //average them to create the twist to publish + msg.linear.x = (desVel + xVel) / 2; + if(msg.linear.x > 0.1){ + msg.linear.x = 0.1; + } else if(msg.linear.x < -0.1){ + msg.linear.x = -0.1; + } + msg.angular.z = 0.1*((desAng + aVel) / 2); + + msg.angular.z = 0.0; + pub_message->publish(msg); + +} + +void localController::pathMessageCallback(const nav_msgs::Path::ConstPtr &msg) +{ + //get a path + + //update current path + currentPath = *msg; + pathCounter = 0; + + ROS_INFO("Got a New Path\n"); +} + +void localController::odomUpdateCallback(const nav_msgs::Odometry::ConstPtr &msg) +{ + //update position in worldFrame + currentVel = msg->twist.twist; + currentPose = msg->pose.pose; + ROS_INFO("Got a New Odom\n"); +} + + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "locol_controller"); + ros::NodeHandle nh; + + localController *local_controller = new localController(); + ros::Publisher pub = nh.advertise("/cmd_vel",10); + ros::Subscriber odom_sub = nh.subscribe("/odom",1000,&localController::odomUpdateCallback,local_controller); + ros::Subscriber path_sub = nh.subscribe("/plan",1000,&localController::pathMessageCallback,local_controller); + ros::Rate r(10); + + while(nh.ok()) + { + local_controller->publishMessage(&pub); + r.sleep(); + ros::spinOnce(); + } + + return 0; +} diff --git a/src/process.m b/src/process.m new file mode 100644 index 0000000..078804f --- /dev/null +++ b/src/process.m @@ -0,0 +1,194 @@ +clear all; +close all; +addpath('rigidTransform') +K(1,1)= 164.255034407511; +K(1,2)= 0.0; +K(1,3)= 214.523999214172; +K(2,1)= 0.0; +K(2,2)= 164.255034407511; +K(2,3)= 119.433252334595; +K(3,1)= 0.0; +K(3,2)= 0.0; +K(3,3)= 1.0; + + +% im1 = im2double(rgb2gray(imread('/Users/zhangchen/Documents/courses/2015 Spring/Robot Autonomy/P2B/cmu_16662_p2/sensor_data/left000.jpg'))); +% im2 = im2double(rgb2gray(imread('/Users/zhangchen/Documents/courses/2015 Spring/Robot Autonomy/P2B/cmu_16662_p2/sensor_data/right000.jpg'))); +% +% [worldPoints1,features1,xy1,sp1] = triangulateIm(im1,im2,K); +% +% im1 = im2double(rgb2gray(imread('/Users/zhangchen/Documents/courses/2015 Spring/Robot Autonomy/P2B/cmu_16662_p2/sensor_data/left100.jpg'))); +% im2 = im2double(rgb2gray(imread('/Users/zhangchen/Documents/courses/2015 Spring/Robot Autonomy/P2B/cmu_16662_p2/sensor_data/right100.jpg'))); +% +% [worldPoints2,features2,xy2,sp2] = triangulateIm(im1,im2,K); +% +% [indexPairs] = matchFeatures(features1,features2,'MaxRatio',.4); +% +% +% wp1 = worldPoints1(1:3,indexPairs(:,1)); +% wp2 = worldPoints2(1:3,indexPairs(:,2)); +% bestT = estimateRigidTransformation(wp1',wp2',5000,0.01); +%THistory = struct('T',bestT); +% % matlabpool open 2 +tstep = 1; + +locList = []; +outfid = fopen('VO_OutputFile.txt', 'wt'); +if outfid < 0 + error('file creation failed'); +end +rtoutfid = fopen('RT_OutputFile.txt', 'wt'); +if rtoutfid < 0 + error('file creation failed'); +end + + +for i = 0:tstep:633 + + im1 = im2double(rgb2gray(imread(sprintf('/home/ed/Documents/robonomy/hw2b/sensor_data/left%03d.jpg',i)))); + im2 = im2double(rgb2gray(imread(sprintf('/home/ed/Documents/robonomy/hw2b/sensor_data/right%03d.jpg',i)))); + + im11 = im1; + [worldPoints1,features1,xy1,xy2,sp1] = triangulateIm(im1,im2,K); + + im1 = im2double(rgb2gray(imread(sprintf('/home/ed/Documents/robonomy/hw2b/sensor_data/left%03d.jpg',i+tstep)))); + im2 = im2double(rgb2gray(imread(sprintf('/home/ed/Documents/robonomy/hw2b/sensor_data/right%03d.jpg',i+tstep)))); + + [worldPoints2,features2,xy12,xy22,sp2] = triangulateIm(im1,im2,K); + + [indexPairs] = matchFeatures(features1,features2,'MaxRatio',.5); + + wp1 = worldPoints1(1:3,indexPairs(:,1)); + wp2 = worldPoints2(1:3,indexPairs(:,2)); + + uv1 = xy1(:,indexPairs(:,1)); + uv2 = xy2(:,indexPairs(:,1)); + +% %make sure features are unique + if(~size(locList)) + locList = features1(indexPairs(:,1),:); + + else + matchedFeatures1 = []; + matchedFeatures1 = features1(indexPairs(:,1),:); + ips = matchFeatures(matchedFeatures1,locList,'MaxRatio',.5); + %add unique ones to locList + fullIDX = 1:size(matchedFeatures1,1); + notIDX = ips(:,1); + idx=setdiff(fullIDX,notIDX); + uniqueFeatures = (matchedFeatures1(idx,:)); + locList = [locList; uniqueFeatures]; + end +% %write to file with indexes + ips = matchFeatures(features1(indexPairs(:,1),:),locList,'MaxRatio',.6); +% +% %x l uL uR v X Y Z + for n = 1:size(ips,1) +% + fwrite(outfid, sprintf('%d %d %f %f %f %f %f %f\n',i,ips(n,2),uv1(1,n),uv2(1,n),uv1(2,n),wp1(1,n),wp1(2,n),wp1(3,n))); + end +% % [bestT bestI] = ransacRigidT(wp1,wp2,xy1(:,indexPairs(:,1)),xy2(:,indexPairs(:,2)),sp1(:,indexPairs(:,1)),sp2(:,indexPairs(:,2)),K,2000,40); +% %wp1size = size(wp1') +% %wp2size = size(wp2') +% bestT = estimateRigidTransformation(wp1',wp2',10000,0.1); +% +% fwrite(rtoutfid, sprintf('%d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n',i,bestT(1,1),bestT(1,2),bestT(1,3),bestT(1,4),bestT(2,1),bestT(2,2),bestT(2,3),bestT(2,4),bestT(3,1),bestT(3,2),bestT(3,3),bestT(3,4),bestT(4,1),bestT(4,2),bestT(4,3),bestT(4,4))); +% +% j = round((i+tstep)/tstep);%(i+5)/5; +% THistory(j) = struct('T',bestT); +% %VOfile{j} = sprintf('%d %d %f %f %f %f %f %f\n',i,ips(n,2),uv1(1,n),uv2(1,n),uv1(2,n),wp1(1,n),wp1(2,n),wp1(3,n)); +% %RTfile{j} = sprintf('%d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n',i,bestT(1,1),bestT(1,2),bestT(1,3),bestT(1,4),bestT(2,1),bestT(2,2),bestT(2,3),bestT(2,4),bestT(3,1),bestT(3,2),bestT(3,3),bestT(3,4),bestT(4,1),bestT(4,2),bestT(4,3),bestT(4,4)); +% %j = j+1; + fprintf('i = %d\n', i); + + + +end + + + +fclose(outfid); +fclose(rtoutfid); + +% currentH = eye(4); +% points = []; +% %figure; hold on; +% figure; hold on +% for i=1:length(THistory) +% t = THistory(i).T(1:4,4); +% +% tDist = sqrt( t(1)^2 + t(2)^2 + t(3)^2); +% if tDist > 0.4 +% % t = t / (tDist / 0.4) +% newT = eye(4); +% else +% newT = [THistory(i).T(:,1:3) t]; +% end +% +% % THistory(i).T +% %pause(01); +% %newT = [THistory(i).T(:,1:3) t]; +% currentH = newT*currentH; +% points = [points currentH*[0;0;0;1]]; +% %currP = currentH*[0;0;0;1]; +% %plot3( +% plotTrajectory(currentH); +% end +% +% points(1,:) = points(1,:)./points(4,:); +% points(2,:) = points(2,:)./points(4,:); +% points(3,:) = points(3,:)./points(4,:); +% +% save('output.mat','THistory','points'); +% +% plot3(points(1,:),points(2,:),points(3,:),'b--o'); +% xlabel('x - axis'); +% ylabel('y - axis'); +% zlabel('z - axis'); +% title('3D Plot of Pose'); + + +% matlabpool close; + + + + +%figure; hold on; +% for i=1:length(THistory) +% t = THistory(i).T(1:4,4); +% +% tDist = sqrt( t(1)^2 + t(2)^2 + t(3)^2); +% if tDist > 400 +% t = t / (tDist / 400) +% +% newT = [THistory(i).T(:,1:3) t]; +% newT = eye(4); +% else +% newT = [THistory(i).T(:,1:3) t]; +% end +% % THistory(i).T +% %pause(01); +% %newT = [THistory(i).T(:,1:3) t]; +% currentH = newT*currentH; +% points = [points currentH*[0;0;0;1]]; +% %currP = currentH*[0;0;0;1]; +% %plot3( +% end +% +% points(1,:) = points(1,:)./points(4,:); +% points(2,:) = points(2,:)./points(4,:); +% points(3,:) = points(3,:)./points(4,:); +% +% save('output.mat','THistory','points'); +% +% figure; hold on; +% plot3(points(1,:),points(2,:),points(3,:),'b--o'); +% xlabel('x'); +% ylabel('y'); +% zlabel('z'); +% % min = 100; +% % max = 200; +% % axis([-1*min,1*max,-1*min,1*max,-1*min,1*max]); +% hold off; +% +% % matlabpool close; diff --git a/src/projmats.yml b/src/projmats.yml new file mode 100644 index 0000000..d1a72ff --- /dev/null +++ b/src/projmats.yml @@ -0,0 +1,17 @@ +%YAML:1.0 + P1: !!opencv-matrix + rows: 3 + cols: 4 + dt: f + data: [ 980.349744, 0.000000, 0.000000, + 0.000000, 0.000000, 1053.853150, 0.000000, + 0.000000, 618.177717, 335.306171, 1.000000, + 0.000000] + P2: !!opencv-matrix + rows: 3 + cols: 4 + dt: f + data: [ 967.267768, -15.421100, 48.204604, + -240912.580941, 17.791754, 1034.707327, -25.994713, + -3100.904162, 639.716227, 354.525991, 23.409509, + -158870.346198] diff --git a/src/projmats.yml~ b/src/projmats.yml~ new file mode 100644 index 0000000..91d2295 --- /dev/null +++ b/src/projmats.yml~ @@ -0,0 +1,17 @@ +%YAML:1.0 + P1: !!opencv-matrix + rows: 3 + cols: 4 + dt: f + data: [ 980.349744, 0.000000, 0.000000, + 0.000000, 0.000000, 1053.853150, 0.000000, + 0.000000, 618.177717, 335.306171, 1.000000, + 0.000000 + P2: !!opencv-matrix + rows: 3 + cols: 4 + dt: f + data: [ 967.267768, -15.421100, 48.204604, + -240912.580941, 17.791754, 1034.707327, -25.994713, + -3100.904162, 639.716227, 354.525991, 23.409509, + -158870.346198 \ No newline at end of file diff --git a/src/standardPoints b/src/standardPoints new file mode 100644 index 0000000..a699518 --- /dev/null +++ b/src/standardPoints @@ -0,0 +1,15 @@ +%YAML:1.0 + leftP: !!opencv-matrix + rows: 4 + cols: 2 + dt: f + data: [ 191.000000, 569.000000, 242.750000, + 403.000000, 1125.750000, 498.750000, 751.250000, + 388.750000] + rightP: !!opencv-matrix + rows: 4 + cols: 2 + dt: f + data: [ 55.000000, 636.000000, 158.750000, + 470.000000, 1012.750000, 541.750000, 684.250000, + 445.750000] diff --git a/src/standardPoints.yml b/src/standardPoints.yml new file mode 100644 index 0000000..eb123fe --- /dev/null +++ b/src/standardPoints.yml @@ -0,0 +1,15 @@ +%YAML:1.0 + leftP: !!opencv-matrix + rows: 4 + cols: 2 + dt: f + data: [ 206.000000, 568.000000, 252.000000, + 409.000000, 1134.000000, 502.000000, 762.000000, + 395.000000] + rightP: !!opencv-matrix + rows: 4 + cols: 2 + dt: f + data: [ 71.000000, 636.000000, 169.000000, + 474.000000, 1018.000000, 545.000000, 689.000000, + 449.000000] diff --git a/src/stereoParameters.mat b/src/stereoParameters.mat new file mode 100644 index 0000000..d319101 Binary files /dev/null and b/src/stereoParameters.mat differ diff --git a/src/template.mat b/src/template.mat new file mode 100644 index 0000000..7975393 Binary files /dev/null and b/src/template.mat differ diff --git a/src/transform3d2d.mat b/src/transform3d2d.mat new file mode 100644 index 0000000..3b2c062 Binary files /dev/null and b/src/transform3d2d.mat differ diff --git a/src/triangulateIm.m b/src/triangulateIm.m new file mode 100644 index 0000000..2c4d4e1 --- /dev/null +++ b/src/triangulateIm.m @@ -0,0 +1,77 @@ +function [worldPoints,matchedFeatures,pts1,pts2,sp1] = triangulateIm(im1,im2,K1,K2,Rt) +Features1 = detectSURFFeatures(im1); +Features2 = detectSURFFeatures(im2); + +[features1, valid_points1] = extractFeatures(im1, Features1); +[features2, valid_points2] = extractFeatures(im2, Features2); + +[indexPairs] = matchFeatures(features1,features2,'MaxRatio',.8); + +matchedPoints1 = valid_points1(indexPairs(:, 1), :); +matchedPoints2 = valid_points2(indexPairs(:, 2), :); + + +%figure; showMatchedFeatures(im1, im2, matchedPoints1, matchedPoints2); + +matchedFeatures = features1(indexPairs(:,1),:); + +pts1 = []; +pts2 = []; + +for i = 1:size(matchedPoints1,1) + temp1 = [round(matchedPoints1(i).Location) 1]'; + pts1 = [pts1 temp1]; + %pts1index = [pts1index indexPairs(i,1)]; + temp2 = [round(matchedPoints2(i).Location) 1]'; + pts2 = [pts2 temp2]; + +end + +R =[1 0 0 0; 0 1 0 0; 0 0 1 0]; +P1 = K1*R; + +R2 =[1 0 0 250; 0 1 0 0; 0 0 1 0]; +P2 = K2*Rt; + + + +% figure; +% hold on; +% +% plot(pts1(1,:),pts1(2,:),'r*'); +% ln = findobj('type','line'); +% +% set(ln,'marker','.','markers',14,'markerfa','b'); +% text(double(pts1(1,:)),double(pts1(2,:)),num2str(linspace(1,size(pts1,2),size(pts1,2)))); + +A = []; +worldPoints = []; +for i=1:size(pts1,2) + A = [pts1(2,i)*P1(3,:)-P1(2,:); + P1(1,:)-pts1(1,i)*P1(3,:); + pts2(2,i)*P2(3,:)-P2(2,:); + P2(1,:)-pts2(1,i)*P2(3,:)]; +% for n=1:4 +% A(n,:) = A(n,:)/norm(A(n,:)); +% end + [U S V] = svd(A); + + h = V(:,end); + rp = P1*h; + worldPoints = [worldPoints h/h(4)]; + sp1 = pts2(1:3,:) - pts1(1:3,:); + %plot(pts1(1,i),pts1(2,i),'r*'); + %text(double(pts1(1,i)),double(pts1(2,i)),num2str(i)); + + %plot(rp(1)/rp(3),rp(2)/rp(3),'ob'); + + +end +%hold off; + +% hold on; +% figure ;plot3(worldPoints(1,:),worldPoints(2,:),worldPoints(3,:),'.g'); +% xlabel('x_c'); +% ylabel('y_c'); +% zlabel('z_c'); +end \ No newline at end of file diff --git a/src/worldpoints.txt b/src/worldpoints.txt new file mode 100644 index 0000000..d9e86dd --- /dev/null +++ b/src/worldpoints.txt @@ -0,0 +1,6 @@ + +[-23.828268, -4.2584815, -36.11837, -30.284458, -23.592314, -26.518307, -32.059162, -29.233158, -26.817089, -24.460331, -23.522896, -27.505138, -31.828316, -24.306587, -24.377087, -24.299513, -23.381594, 10.861652, 8.7567215, 12.706574; + 62.522312, 51.112339, 68.323669, 66.115982, 62.499329, 64.04644, 66.325447, 65.044228, 63.901974, 63.36002, 62.531288, 64.265991, 66.207573, 62.707153, 63.538361, 63.081623, 62.692768, 43.289967, 42.044323, 45.128002; + 5461.0645, 4737.8267, 5906.6777, 5715.0752, 5458.1167, 5567.8101, 5755.9717, 5657.0903, 5596.4565, 5500.959, 5467.9824, 5590.2002, 5741.2915, 5503.7305, 5502.084, 5501.2671, 5468.9321, 4502.1396, 4619.084, 4261.6152; + 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] + diff --git a/src/worldpoints2.txt b/src/worldpoints2.txt new file mode 100644 index 0000000..a1874e0 --- /dev/null +++ b/src/worldpoints2.txt @@ -0,0 +1,4 @@ +[1.7723925, 8.1316299, 10.918985, 13.134124; + 42.105042, 45.241249, 47.286522, 46.736198; + 5089.2983, 4696.4893, 4331.2734, 4262.2104] +