diff --git a/face_detector/CMakeLists.txt b/face_detector/CMakeLists.txt index 7a75709a..5e1f23e5 100644 --- a/face_detector/CMakeLists.txt +++ b/face_detector/CMakeLists.txt @@ -25,13 +25,6 @@ find_package(catkin REQUIRED COMPONENTS find_package(Boost REQUIRED COMPONENTS signals system thread) find_package(OpenCV) -include_directories( - include - ${Boost_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - add_action_files( DIRECTORY action FILES FaceDetector.action @@ -59,33 +52,44 @@ catkin_package( tf ) +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + add_executable(face_detector src/face_detection.cpp src/faces.cpp) -target_link_libraries(${PROJECT_NAME} +add_dependencies(face_detector ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(face_detector ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBRARIES}) -add_dependencies(face_detector ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -install(TARGETS - face_detector - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +install(TARGETS face_detector RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) - -install(DIRECTORY include/face_detector/ +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -install(DIRECTORY launch param +install(DIRECTORY param + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ +) +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ ) +catkin_install_python(PROGRAMS scripts/face_detector_action_client.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) + find_package(roslint REQUIRED) + find_package(roslaunch REQUIRED) catkin_download_test_data(${PROJECT_NAME}_noface_test_diamondback.bag http://download.ros.org/data/face_detector/face_detector_noface_test_diamondback.bag @@ -98,8 +102,8 @@ if(CATKIN_ENABLE_TESTING) # add_rostest(test/wide-stereo_true_rtest.xml) # add_rostest(test/wide-stereo_false_rtest.xml) - add_rostest(test/rgbd_true_rtest.xml) - add_rostest(test/rgbd_false_rtest.xml) +# add_rostest(test/rgbd_true_rtest.xml) +# add_rostest(test/rgbd_false_rtest.xml) # add_rostest(test/narrow-stereo_true_rtest.xml) # add_rostest(test/narrow-stereo_false_rtest.xml) # add_rostest(test/action-wide_true_rtest.xml) @@ -107,4 +111,8 @@ if(CATKIN_ENABLE_TESTING) # add_rostest(test/action-rgbd_true_rtest.xml) # add_rostest(test/action-rgbd_false_rtest.xml) + roslint_cpp() + roslaunch_add_file_check(launch) + roslint_python() + roslint_add_test() endif() diff --git a/face_detector/include/face_detector/faces.h b/face_detector/include/face_detector/faces.h index cecb6abf..9eb5b387 100644 --- a/face_detector/include/face_detector/faces.h +++ b/face_detector/include/face_detector/faces.h @@ -38,36 +38,31 @@ /* Author: Caroline Pantofaru */ -#ifndef FACES_H -#define FACES_H - - +#ifndef FACE_DETECTOR_FACES_H +#define FACE_DETECTOR_FACES_H +#include #include #include #include #include -#include "image_geometry/stereo_camera_model.h" +#include #include #include #include #include - -using namespace std; - #define FACE_SIZE_MIN_M 0.1 /**< Default minimum face size, in meters. Only use this for initialization. */ #define FACE_SIZE_MAX_M 0.5 /**< Default maximum face size, in meters. Only use this for initialization. */ -#define MAX_FACE_Z_M 8.0 /**< Default maximum distance from the camera, in meters. Only use this for initialization. */ +#define MAX_FACE_Z_M 8.0 /**< Default maximum distance from the camera, in meters. Only use for initialization. */ // Default thresholds for face tracking. #define FACE_SEP_DIST_M 1.0 /**< Default separation distance for associating faces. Only use this for initialization. */ namespace people { - /*! * \brief A structure for holding information about boxes in 2d and 3d. */ @@ -78,30 +73,26 @@ struct Box2D3D double width2d; double width3d; cv::Rect box2d; - string status; + std::string status; int id; }; - /*! * \brief A structure containing the person's identifying data. */ struct Face { - string id; - string name; + std::string id; + std::string name; }; - /*! * \brief Contains a list of faces and functions that can be performed on that list. * This includes utility tasks such as set/get data, to more complicated tasks such as detection or tracking. */ - class Faces { public: - // Default thresholds for the face detection algorithm. // Thresholds for the face detection algorithm. @@ -111,15 +102,12 @@ class Faces // Thresholds for face tracking. double face_sep_dist_m_; /**< Separation distance for associating faces. */ - - // Create an empty list of people. Faces(); // Destroy a list of people. ~Faces(); - /*! * \brief Detect all faces in an image and disparity image. * @@ -132,7 +120,8 @@ class Faces * Output: * A vector of Box2D3Ds containing the bounding boxes around found faces in 2D and 3D. */ - vector detectAllFacesDisparity(const cv::Mat &image, double threshold, const cv::Mat &disparity_image, image_geometry::StereoCameraModel *cam_model); + std::vector detectAllFacesDisparity(const cv::Mat &image, double threshold, const cv::Mat &disparity_image, + image_geometry::StereoCameraModel *cam_model); /*! * \brief Detect all faces in an image and depth image. @@ -146,7 +135,8 @@ class Faces * Output: * A vector of Box2D3Ds containing the bounding boxes around found faces in 2D and 3D. */ - vector detectAllFacesDepth(const cv::Mat &image, double threshold, const cv::Mat &depth_image, image_geometry::StereoCameraModel *cam_model); + std::vector detectAllFacesDepth(const cv::Mat &image, double threshold, const cv::Mat &depth_image, + image_geometry::StereoCameraModel *cam_model); /*! * \brief Initialize the face detector with images and disparities. @@ -156,8 +146,8 @@ class Faces * \param num_cascades Should always be 1 (may change in the future.) * \param haar_classifier_filename Full path to the cascade file. */ - void initFaceDetectionDisparity(uint num_cascades, string haar_classifier_filename, double face_size_min_m, double face_size_max_m, double max_face_z_m, double face_sep_dist_m); - + void initFaceDetectionDisparity(uint num_cascades, std::string haar_classifier_filename, double face_size_min_m, + double face_size_max_m, double max_face_z_m, double face_sep_dist_m); /*! * \brief Initialize the face detector with images and depth. @@ -167,13 +157,13 @@ class Faces * \param num_cascades Should always be 1 (may change in the future.) * \param haar_classifier_filename Full path to the cascade file. */ - void initFaceDetectionDepth(uint num_cascades, string haar_classifier_filename, double face_size_min_m, double face_size_max_m, double max_face_z_m, double face_sep_dist_m); + void initFaceDetectionDepth(uint num_cascades, std::string haar_classifier_filename, double face_size_min_m, + double face_size_max_m, double max_face_z_m, double face_sep_dist_m); //////////////////// private: - - vector list_; /**< The list of face ids. */ - vector faces_; /**< The list of face positions. */ + std::vector list_; /**< The list of face ids. */ + std::vector faces_; /**< The list of face positions. */ cv::Mat cv_image_gray_; /**< Grayscale image */ cv::Mat const* disparity_image_; /**< Disparity image */ @@ -192,10 +182,7 @@ class Faces void faceDetectionThreadDisparity(uint i); void faceDetectionThreadDepth(uint i); - -}; - }; +} // namespace people -#endif - +#endif // FACE_DETECTOR_FACES_H diff --git a/face_detector/package.xml b/face_detector/package.xml index 7e1c4c74..21edeb14 100644 --- a/face_detector/package.xml +++ b/face_detector/package.xml @@ -17,6 +17,7 @@ geometry_msgs image_geometry image_transport + message_filters people_msgs rosbag roscpp @@ -25,10 +26,9 @@ sensor_msgs std_msgs std_srvs + stereo_image_proc stereo_msgs tf - message_filters - stereo_image_proc message_generation message_runtime @@ -36,6 +36,8 @@ dynamic_reconfigure message_runtime - stereo_image_proc + roslaunch + roslint rostest + stereo_image_proc diff --git a/face_detector/scripts/face_detector_action_client.py b/face_detector/scripts/face_detector_action_client.py index 6c824248..91133bd9 100755 --- a/face_detector/scripts/face_detector_action_client.py +++ b/face_detector/scripts/face_detector_action_client.py @@ -1,25 +1,22 @@ #! /usr/bin/env python -import roslib; roslib.load_manifest('face_detector') -import rospy - -# Brings in the SimpleActionClient import actionlib -# Brings in the messages used by the face_detector action, including the -# goal message and the result message. -import face_detector.msg +from face_detector.msg import FaceDetectorAction, FaceDetectorGoal + +import rospy + def face_detector_client(): # Creates the SimpleActionClient, passing the type of the action to the constructor. - client = actionlib.SimpleActionClient('face_detector_action', face_detector.msg.FaceDetectorAction) + client = actionlib.SimpleActionClient('face_detector_action', FaceDetectorAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() - # Creates a goal to send to the action server. - goal = face_detector.msg.FaceDetectorGoal() + # Creates a goal to send to the action server. (no parameters) + goal = FaceDetectorGoal() # Sends the goal to the action server. client.send_goal(goal) @@ -27,8 +24,8 @@ def face_detector_client(): # Waits for the server to finish performing the action. client.wait_for_result() - # Prints out the result of executing the action - return client.get_result() # A FibonacciResult + return client.get_result() # people_msgs/PositionMeasurement[] face_positions + if __name__ == '__main__': try: @@ -36,6 +33,6 @@ def face_detector_client(): # publish and subscribe over ROS. rospy.init_node('face_detector_action_client') result = face_detector_client() - print "Done action" + print('Done action') except rospy.ROSInterruptException: - print "Program interrupted before completion" + print('Program interrupted before completion') diff --git a/face_detector/src/face_detection.cpp b/face_detector/src/face_detection.cpp index 1779b8ee..3577febf 100644 --- a/face_detector/src/face_detection.cpp +++ b/face_detector/src/face_detection.cpp @@ -35,12 +35,13 @@ /* Author: Caroline Pantofaru */ - - #include #include #include #include +#include +#include +#include #include #include @@ -54,28 +55,26 @@ #include #include #include -#include "sensor_msgs/CameraInfo.h" -#include "sensor_msgs/Image.h" -#include "stereo_msgs/DisparityImage.h" -#include "sensor_msgs/image_encodings.h" -#include "cv_bridge/cv_bridge.h" -#include "tf/transform_listener.h" -#include "sensor_msgs/PointCloud.h" -#include "geometry_msgs/Point32.h" +#include +#include +#include +#include +#include +#include +#include +#include -#include "opencv/cxcore.hpp" -#include "opencv/cv.hpp" -#include "opencv/highgui.h" +#include +#include +#include -#include "face_detector/faces.h" +#include -#include "image_geometry/stereo_camera_model.h" +#include #include #include -using namespace std; - namespace people { @@ -85,7 +84,7 @@ class FaceDetector { public: // Constants - const double BIGDIST_M;// = 1000000.0; + const double BIGDIST_M; // = 1000000.0; // Node handle ros::NodeHandle nh_; @@ -94,22 +93,22 @@ class FaceDetector bool use_rgbd_; // Subscription topics - string image_image_; + std::string image_image_; // Stereo - string stereo_namespace_; - string left_topic_; - string disparity_topic_; - string left_camera_info_topic_; - string right_camera_info_topic_; + std::string stereo_namespace_; + std::string left_topic_; + std::string disparity_topic_; + std::string left_camera_info_topic_; + std::string right_camera_info_topic_; // RGB-D - string camera_; - string camera_topic_; - string depth_image_; - string depth_topic_; - string camera_info_topic_; - string depth_info_topic_; - string rgb_ns_; - string depth_ns_; + std::string camera_; + std::string camera_topic_; + std::string depth_image_; + std::string depth_topic_; + std::string camera_info_topic_; + std::string depth_info_topic_; + std::string rgb_ns_; + std::string depth_ns_; // Images and conversion for both the stereo camera and rgb-d camera cases. image_transport::ImageTransport it_; @@ -120,16 +119,24 @@ class FaceDetector message_filters::Subscriber c2_info_sub_; /**< Right/depth camera info msg. */ // Disparity: - typedef message_filters::sync_policies::ExactTime ExactDispPolicy; /**< Sync policy for exact time with disparity. */ - typedef message_filters::sync_policies::ApproximateTime ApproximateDispPolicy; /**< Sync policy for approx time with disparity. */ + typedef message_filters::sync_policies::ExactTime + + ExactDispPolicy; /**< Sync policy for exact time with disparity. */ + typedef message_filters::sync_policies::ApproximateTime + + ApproximateDispPolicy; /**< Sync policy for approx time with disparity. */ typedef message_filters::Synchronizer ExactDispSync; typedef message_filters::Synchronizer ApproximateDispSync; boost::shared_ptr exact_disp_sync_; boost::shared_ptr approximate_disp_sync_; // Depth: - typedef message_filters::sync_policies::ExactTime ExactDepthPolicy; /**< Sync policy for exact time with depth. */ - typedef message_filters::sync_policies::ApproximateTime ApproximateDepthPolicy; /**< Sync policy for approx time with depth. */ + typedef message_filters::sync_policies::ExactTime + + ExactDepthPolicy; /**< Sync policy for exact time with depth. */ + typedef message_filters::sync_policies::ApproximateTime + + ApproximateDepthPolicy; /**< Sync policy for approx time with depth. */ typedef message_filters::Synchronizer ExactDepthSync; typedef message_filters::Synchronizer ApproximateDepthSync; boost::shared_ptr exact_depth_sync_; @@ -140,12 +147,12 @@ class FaceDetector face_detector::FaceDetectorFeedback feedback_; face_detector::FaceDetectorResult result_; - // If running the face detector as a component in part of a larger person tracker, this subscribes to the tracker's position measurements and whether it was initialized by some other node. + // If running the face detector as a component in part of a larger person tracker, + // this subscribes to the tracker's position measurements and whether it was initialized by some other node. // Todo: resurrect the person tracker. ros::Subscriber pos_sub_; bool external_init_; - // Publishers // A point cloud of the face positions, meant for visualization in rviz. // This could be replaced by visualization markers, but they can't be modified @@ -163,8 +170,8 @@ class FaceDetector // Face detector params and output Faces *faces_; /**< List of faces and associated fcns. */ - string name_; /**< Name of the detector. Ie frontalface, profileface. These will be the names in the published face location msgs. */ - string haar_filename_; /**< Training file for the haar cascade classifier. */ + std::string name_; /**< Name of the detector (frontalface, profileface, etc) to be published in face location msgs. */ + std::string haar_filename_; /**< Training file for the haar cascade classifier. */ double reliability_; /**< Reliability of the predictions. This should depend on the training file used. */ struct RestampedPositionMeasurement @@ -173,7 +180,7 @@ class FaceDetector people_msgs::PositionMeasurement pos; double dist; }; - map pos_list_; /**< Queue of updated face positions from the filter. */ + std::map pos_list_; /**< Queue of updated face positions from the filter.*/ int max_id_; @@ -185,11 +192,12 @@ class FaceDetector boost::mutex cv_mutex_, pos_mutex_, limage_mutex_, dimage_mutex_; - bool do_continuous_; /**< True = run as a normal node, searching for faces continuously, False = run as an action, wait for action call to start detection. */ + bool do_continuous_; /**< True = search for faces continuously. False = run as an action. */ - bool do_publish_unknown_; /**< Publish faces even if they have unknown depth/size. Will just use the image x,y in the pos field of the published position_measurement. */ + bool do_publish_unknown_; /**< Publish faces even if they have unknown depth/size. */ + // Will just use the image x, y in the pos field of the published position_measurement. - FaceDetector(std::string name) : + explicit FaceDetector(std::string name) : BIGDIST_M(1000000.0), it_(nh_), as_(nh_, name, false), @@ -236,8 +244,8 @@ class FaceDetector // Init the detector and subscribe to the images and camera parameters. One case for rgbd, one for stereo. if (use_rgbd_) { - - faces_->initFaceDetectionDepth(1, haar_filename_, face_size_min_m, face_size_max_m, max_face_z_m, face_sep_dist_m); + faces_->initFaceDetectionDepth(1, haar_filename_, + face_size_min_m, face_size_max_m, max_face_z_m, face_sep_dist_m); camera_ = nh_.resolveName("camera"); image_image_ = nh_.resolveName("image_topic"); @@ -270,11 +278,11 @@ class FaceDetector exact_depth_sync_->registerCallback(boost::bind(&FaceDetector::imageCBAllDepth, this, _1, _2, _3, _4)); } - } else { - faces_->initFaceDetectionDisparity(1, haar_filename_, face_size_min_m, face_size_max_m, max_face_z_m, face_sep_dist_m); + faces_->initFaceDetectionDisparity(1, haar_filename_, + face_size_min_m, face_size_max_m, max_face_z_m, face_sep_dist_m); stereo_namespace_ = nh_.resolveName("camera"); image_image_ = nh_.resolveName("image_topic"); @@ -305,29 +313,29 @@ class FaceDetector ros::SubscriberStatusCallback pos_pub_connect_cb = boost::bind(&FaceDetector::connectCb, this); ros::SubscriberStatusCallback cloud_pub_connect_cb = boost::bind(&FaceDetector::connectCb, this); if (do_continuous_) - ROS_INFO_STREAM_NAMED("face_detector", "You must subscribe to one of FaceDetector's outbound topics or else it will not publish anything."); + ROS_INFO_STREAM_NAMED("face_detector", "You must subscribe to one of FaceDetector's outbound topics " + "or else it will not publish anything."); { boost::mutex::scoped_lock lock(connect_mutex_); // Advertise a position measure message. - pos_array_pub_ = nh_.advertise("face_detector/people_tracker_measurements_array", 1, pos_pub_connect_cb, pos_pub_connect_cb); + pos_array_pub_ = nh_.advertise( + "face_detector/people_tracker_measurements_array", 1, pos_pub_connect_cb, pos_pub_connect_cb); - cloud_pub_ = nh_.advertise("face_detector/faces_cloud", 0, cloud_pub_connect_cb, cloud_pub_connect_cb); + cloud_pub_ = nh_.advertise("face_detector/faces_cloud", 0, + cloud_pub_connect_cb, cloud_pub_connect_cb); } - // If running as an action server, just stay connected so that action calls are fast. if (!do_continuous_) connectCb(); ros::MultiThreadedSpinner s(2); ros::spin(s); - } ~FaceDetector() { - cv_image_out_.release(); if (do_display_) @@ -350,7 +358,8 @@ class FaceDetector { if (do_continuous_ && cloud_pub_.getNumSubscribers() == 0 && pos_array_pub_.getNumSubscribers() == 0) { - ROS_INFO_STREAM_NAMED("face_detector", "You have unsubscribed to FaceDetector's outbound topics, so it will no longer publish anything."); + ROS_INFO_STREAM_NAMED("face_detector", "You have unsubscribed to FaceDetector's outbound topics, " + "so it will no longer publish anything."); image_sub_.unsubscribe(); depth_image_sub_.unsubscribe(); c1_info_sub_.unsubscribe(); @@ -366,7 +375,8 @@ class FaceDetector // // Subscribe to filter measurements. // if (external_init_) { // //pos_sub_ = nh_.subscribe("people_tracker_filter",1,&FaceDetector::posCallback,this); - // pos_sub_ = nh_.subscribe("/face_detector/people_tracker_measurements_array",1,&FaceDetector::posCallback,this); + // pos_sub_ = nh_.subscribe("/face_detector/people_tracker_measurements_array", + // 1,&FaceDetector::posCallback,this); // } } } @@ -374,7 +384,8 @@ class FaceDetector { if (do_continuous_ && cloud_pub_.getNumSubscribers() == 0 && pos_array_pub_.getNumSubscribers() == 0) { - ROS_INFO_STREAM_NAMED("face_detector", "You have unsubscribed to FaceDetector's outbound topics, so it will no longer publish anything."); + ROS_INFO_STREAM_NAMED("face_detector", "You have unsubscribed to FaceDetector's outbound topics, " + "so it will no longer publish anything."); image_sub_.unsubscribe(); disp_image_sub_.unsubscribe(); c1_info_sub_.unsubscribe(); @@ -390,13 +401,13 @@ class FaceDetector // // Subscribe to filter measurements. // if (external_init_) { // //pos_sub_ = nh_.subscribe("people_tracker_filter",1,&FaceDetector::posCallback,this); - // pos_sub_ = nh_.subscribe("/face_detector/people_tracker_measurements_array",1,&FaceDetector::posCallback,this); + // pos_sub_ = nh_.subscribe("/face_detector/people_tracker_measurements_array",1, + // &FaceDetector::posCallback,this); // } } } } - void goalCB() { as_.acceptNewGoal(); @@ -407,7 +418,6 @@ class FaceDetector as_.setPreempted(); } - /*! * \brief Position message callback. * @@ -419,7 +429,7 @@ class FaceDetector // // Put the incoming position into the position queue. It'll be processed in the next image callback. // boost::mutex::scoped_lock lock(pos_mutex_); - // map::iterator it; + // std::map::iterator it; // for (uint ipa = 0; ipa < pos_array_ptr->people.size(); ipa++) { // it = pos_list_.find(pos_array_ptr->people[ipa].object_id); // RestampedPositionMeasurement rpm; @@ -427,9 +437,10 @@ class FaceDetector // rpm.restamp = pos_array_ptr->people[ipa].header.stamp; // rpm.dist = BIGDIST_M; // if (it == pos_list_.end()) { - // pos_list_.insert(pair(pos_array_ptr->people[ipa].object_id, rpm)); + // pos_list_.insert(pair(pos_array_ptr->people[ipa].object_id, rpm)); // } - // else if ((pos_array_ptr->people[ipa].header.stamp - (*it).second.pos.header.stamp) > ros::Duration().fromSec(-1.0) ){ + // else if ((pos_array_ptr->people[ipa].header.stamp - (*it).second.pos.header.stamp) > + // ros::Duration().fromSec(-1.0) ){ // (*it).second = rpm; // } // } @@ -449,11 +460,14 @@ class FaceDetector * For each new image: * convert it to OpenCV format, perform face detection using OpenCV's haar filter cascade classifier, and * (if requested) draw rectangles around the found faces. - * Can also compute which faces are associated (by proximity, currently) with faces it already has in its list of people. + * Can also compute which faces are associated (by proximity, currently) + * with faces it already has in its list of people. */ - void imageCBAllDepth(const sensor_msgs::Image::ConstPtr &image, const sensor_msgs::Image::ConstPtr& depth_image, const sensor_msgs::CameraInfo::ConstPtr& c1_info, const sensor_msgs::CameraInfo::ConstPtr& c2_info) + void imageCBAllDepth(const sensor_msgs::Image::ConstPtr &image, + const sensor_msgs::Image::ConstPtr& depth_image, + const sensor_msgs::CameraInfo::ConstPtr& c1_info, + const sensor_msgs::CameraInfo::ConstPtr& c2_info) { - // Only run the detector if in continuous mode or the detector was turned on through an action invocation. if (!do_continuous_ && !as_.isActive()) return; @@ -475,8 +489,6 @@ class FaceDetector cv_depth_ptr->image.convertTo(depth_32fc1, CV_32FC1, 0.001); } - - cam_model_.fromCameraInfo(c1_info, c2_info); // For display, keep a copy of the image that we can draw on. @@ -489,7 +501,7 @@ class FaceDetector gettimeofday(&timeofday, NULL); ros::Time starttdetect = ros::Time().fromNSec(1e9 * timeofday.tv_sec + 1e3 * timeofday.tv_usec); - vector faces_vector = faces_->detectAllFacesDepth(cv_image_ptr->image, 1.0, depth_32fc1, &cam_model_); + std::vector faces_vector = faces_->detectAllFacesDepth(cv_image_ptr->image, 1.0, depth_32fc1, &cam_model_); gettimeofday(&timeofday, NULL); ros::Time endtdetect = ros::Time().fromNSec(1e9 * timeofday.tv_sec + 1e3 * timeofday.tv_usec); ros::Duration diffdetect = endtdetect - starttdetect; @@ -506,9 +518,11 @@ class FaceDetector * (if requested) draw rectangles around the found faces. * Can also compute which faces are associated (by proximity, currently) with faces it already has in its list of people. */ - void imageCBAllDisp(const sensor_msgs::Image::ConstPtr &image, const stereo_msgs::DisparityImage::ConstPtr& disp_image, const sensor_msgs::CameraInfo::ConstPtr& c1_info, const sensor_msgs::CameraInfo::ConstPtr& c2_info) + void imageCBAllDisp(const sensor_msgs::Image::ConstPtr &image, + const stereo_msgs::DisparityImage::ConstPtr& disp_image, + const sensor_msgs::CameraInfo::ConstPtr& c1_info, + const sensor_msgs::CameraInfo::ConstPtr& c2_info) { - // Only run the detector if in continuous mode or the detector was turned on through an action invocation. if (!do_continuous_ && !as_.isActive()) return; @@ -521,7 +535,6 @@ class FaceDetector cv_mutex_.lock(); } - // ROS --> OpenCV cv_bridge::CvImageConstPtr cv_image_ptr = cv_bridge::toCvShare(image, sensor_msgs::image_encodings::BGR8); cv_bridge::CvImageConstPtr cv_disp_ptr = cv_bridge::toCvShare(disp_image->image, disp_image); @@ -537,7 +550,8 @@ class FaceDetector gettimeofday(&timeofday, NULL); ros::Time starttdetect = ros::Time().fromNSec(1e9 * timeofday.tv_sec + 1e3 * timeofday.tv_usec); - vector faces_vector = faces_->detectAllFacesDisparity(cv_image_ptr->image, 1.0, cv_disp_ptr->image, &cam_model_); + std::vector faces_vector = faces_->detectAllFacesDisparity(cv_image_ptr->image, 1.0, + cv_disp_ptr->image, &cam_model_); gettimeofday(&timeofday, NULL); ros::Time endtdetect = ros::Time().fromNSec(1e9 * timeofday.tv_sec + 1e3 * timeofday.tv_usec); ros::Duration diffdetect = endtdetect - starttdetect; @@ -546,10 +560,8 @@ class FaceDetector matchAndDisplay(faces_vector, image->header); } - private: - - void matchAndDisplay(vector faces_vector, std_msgs::Header header) + void matchAndDisplay(std::vector faces_vector, std_msgs::Header header) { bool found_faces = false; @@ -560,10 +572,9 @@ class FaceDetector if (faces_vector.size() > 0) { - // Transform the positions of the known faces and remove anyone who hasn't had an update in a long time. // boost::mutex::scoped_lock pos_lock(pos_mutex_); - map::iterator it; + std::map::iterator it; it = pos_list_.begin(); while (it != pos_list_.end()) { @@ -605,7 +616,6 @@ class FaceDetector if (one_face->status == "good" || (one_face->status == "unknown" && do_publish_unknown_)) { - std::string id = ""; // Convert the face format to a PositionMeasurement msg. @@ -614,9 +624,9 @@ class FaceDetector pos.pos.x = one_face->center3d.x; pos.pos.y = one_face->center3d.y; pos.pos.z = one_face->center3d.z; - pos.header.frame_id = header.frame_id;//"*_optical_frame"; + pos.header.frame_id = header.frame_id; // "*_optical_frame"; pos.reliability = reliability_; - pos.initialization = 1;//0; + pos.initialization = 1; // 0; pos.covariance[0] = 0.04; pos.covariance[1] = 0.0; pos.covariance[2] = 0.0; @@ -627,14 +637,18 @@ class FaceDetector pos.covariance[7] = 0.0; pos.covariance[8] = 0.04; - // Check if this person's face is close enough to one of the previously known faces and associate it with the closest one. + // Check if this person's face is close enough to one of the previously known faces + // and associate it with the closest one. // Otherwise publish it with an empty id. - // Note that multiple face positions can be published with the same id, but ids in the pos_list_ are unique. The position of a face in the list is updated with the closest found face. + // Note that multiple face positions can be published with the same id, but ids in the pos_list_ are unique. + // The position of a face in the list is updated with the closest found face. double dist, mindist = BIGDIST_M; - map::iterator close_it = pos_list_.end(); + std::map::iterator close_it = pos_list_.end(); for (it = pos_list_.begin(); it != pos_list_.end(); it++) { - dist = pow((*it).second.pos.pos.x - pos.pos.x, 2.0) + pow((*it).second.pos.pos.y - pos.pos.y, 2.0) + pow((*it).second.pos.pos.z - pos.pos.z, 2.0); + dist = pow((*it).second.pos.pos.x - pos.pos.x, 2.0) + + pow((*it).second.pos.pos.y - pos.pos.y, 2.0) + + pow((*it).second.pos.pos.z - pos.pos.z, 2.0); if (dist <= faces_->face_sep_dist_m_ && dist < mindist) { mindist = dist; @@ -655,14 +669,12 @@ class FaceDetector else { max_id_++; - pos.object_id = static_cast(&(ostringstream() << max_id_))->str(); + pos.object_id = static_cast(&(std::ostringstream() << max_id_))->str(); ROS_INFO_STREAM_NAMED("face_detector", "Didn't find face to match, starting new ID " << pos.object_id); } result_.face_positions.push_back(pos); found_faces = true; - } - } pos_array.header.stamp = header.stamp; pos_array.header.frame_id = header.frame_id; @@ -672,7 +684,7 @@ class FaceDetector pos_array_pub_.publish(pos_array); // Update the position list greedily. This should be rewritten. - map::iterator it; + std::map::iterator it; for (uint ipa = 0; ipa < pos_array.people.size(); ipa++) { it = pos_list_.find(pos_array.people[ipa].object_id); @@ -682,7 +694,8 @@ class FaceDetector rpm.dist = BIGDIST_M; if (it == pos_list_.end()) { - pos_list_.insert(pair(pos_array.people[ipa].object_id, rpm)); + pos_list_.insert(std::pair(pos_array.people[ipa].object_id, + rpm)); } else if ((pos_array.people[ipa].header.stamp - (*it).second.pos.header.stamp) > ros::Duration().fromSec(-1.0)) { @@ -696,16 +709,13 @@ class FaceDetector { (*it).second.dist = BIGDIST_M; } - } - // Done associating faces /******** Display **************************************************************/ // Publish a point cloud of face centers. - cloud.channels.resize(1); cloud.channels[0].name = "intensity"; @@ -716,7 +726,6 @@ class FaceDetector // Visualization of good faces as a point cloud if (one_face->status == "good") { - geometry_msgs::Point32 p; p.x = one_face->center3d.x; p.y = one_face->center3d.y; @@ -724,15 +733,13 @@ class FaceDetector cloud.points.push_back(p); cloud.channels[0].values.push_back(1.0f); - ngood ++; + ngood++; } } cloud_pub_.publish(cloud); // Done publishing the point cloud. - - } // Done if faces_vector.size() > 0 - + } // Done if faces_vector.size() > 0 // Draw an appropriately colored rectangle on the display image and in the visualizer. if (do_display_) @@ -743,8 +750,8 @@ class FaceDetector // Done drawing. /******** Done display **********************************************************/ - - ROS_INFO_STREAM_NAMED("face_detector", "Number of faces found: " << faces_vector.size() << ", number with good depth and size: " << ngood); + ROS_INFO_STREAM_NAMED("face_detector", "Number of faces found: " << faces_vector.size() << + ", number with good depth and size: " << ngood); // If you don't want continuous processing and you've found at least one face, turn off the detector. if (!do_continuous_ && found_faces) @@ -754,9 +761,8 @@ class FaceDetector } // Draw bounding boxes around detected faces on the cv_image_out_ and show the image. - void displayFacesOnImage(vector faces_vector) + void displayFacesOnImage(std::vector faces_vector) { - Box2D3D *one_face; for (uint iface = 0; iface < faces_vector.size(); iface++) @@ -781,19 +787,17 @@ class FaceDetector cv::rectangle(cv_image_out_, cv::Point(one_face->box2d.x, one_face->box2d.y), - cv::Point(one_face->box2d.x + one_face->box2d.width, one_face->box2d.y + one_face->box2d.height), color, 4); + cv::Point(one_face->box2d.x + one_face->box2d.width, + one_face->box2d.y + one_face->box2d.height), + color, 4); } } cv::imshow("Face detector: Face Detection", cv_image_out_); cv::waitKey(2); - } - - -}; // end class - -}; // end namespace people +}; // end class +}; // end namespace people // Main int main(int argc, char **argv) diff --git a/face_detector/src/faces.cpp b/face_detector/src/faces.cpp index 442c838e..6ed6a21c 100644 --- a/face_detector/src/faces.cpp +++ b/face_detector/src/faces.cpp @@ -38,7 +38,7 @@ /* Author: Caroline Pantofaru */ -#include "face_detector/faces.h" +#include #include #include #include @@ -46,11 +46,14 @@ #include #include +#include +#include + namespace people { Faces::Faces(): - list_(NULL), + list_(), cam_model_(NULL) { } @@ -69,18 +72,16 @@ Faces::~Faces() } cam_model_ = 0; - } - /* Note: The multi-threading in this file is left over from a previous incarnation that allowed multiple * cascades to be run at once. It hasn't been removed in case we want to return to that model, and since * there isn't much overhead. Right now, however, only one classifier is being run per instantiation * of the face_detector node. */ - -void Faces::initFaceDetectionDisparity(uint num_cascades, string haar_classifier_filename, double face_size_min_m, double face_size_max_m, double max_face_z_m, double face_sep_dist_m) +void Faces::initFaceDetectionDisparity(uint num_cascades, std::string haar_classifier_filename, double face_size_min_m, + double face_size_max_m, double max_face_z_m, double face_sep_dist_m) { images_ready_ = 0; @@ -101,14 +102,13 @@ void Faces::initFaceDetectionDisparity(uint num_cascades, string haar_classifier } ///// - -vector Faces::detectAllFacesDisparity(const cv::Mat &image, double threshold, const cv::Mat &disparity_image, image_geometry::StereoCameraModel *cam_model) +std::vector Faces::detectAllFacesDisparity(const cv::Mat &image, double threshold, + const cv::Mat &disparity_image, + image_geometry::StereoCameraModel *cam_model) { - faces_.clear(); // Convert the image to grayscale, if necessary. - if (image.channels() == 1) { cv_image_gray_.create(image.size(), CV_8UC1); @@ -146,15 +146,13 @@ vector Faces::detectAllFacesDisparity(const cv::Mat &image, double thre } ///// - void Faces::faceDetectionThreadDisparity(uint i) { - - while (1) + while (true) { boost::mutex::scoped_lock fgmlock(*(face_go_mutex_)); boost::mutex::scoped_lock tlock(t_mutex_, boost::defer_lock); - while (1) + while (true) { tlock.lock(); if (images_ready_) @@ -172,36 +170,42 @@ void Faces::faceDetectionThreadDisparity(uint i) cv::Point3d p3_2(face_size_min_m_, 0, max_face_z_m_); cv::Point2d p2_1 = (cam_model_->left()).project3dToPixel(p3_1); cv::Point2d p2_2 = (cam_model_->left()).project3dToPixel(p3_2); - int this_min_face_size = (int)(floor(fabs(p2_2.x - p2_1.x))); + int this_min_face_size = static_cast(floor(fabs(p2_2.x - p2_1.x))); std::vector faces_vec; - cascade_.detectMultiScale(cv_image_gray_, faces_vec, 1.2, 2, CV_HAAR_DO_CANNY_PRUNING, cv::Size(this_min_face_size, this_min_face_size)); + cascade_.detectMultiScale(cv_image_gray_, faces_vec, 1.2, 2, CV_HAAR_DO_CANNY_PRUNING, + cv::Size(this_min_face_size, this_min_face_size)); - // Filter the faces using depth information, if available. Currently checks that the actual face size is within the given limits. + // Filter the faces using depth information, if available. + // Currently checks that the actual face size is within the given limits. cv::Scalar color(0, 255, 0); Box2D3D one_face; double avg_disp = 0.0; cv::Mat uvd(1, 3, CV_32FC1); cv::Mat xyz(1, 3, CV_32FC1); // For each face... - for (uint iface = 0; iface < faces_vec.size(); iface++) //face_seq->total; iface++) { + for (uint iface = 0; iface < faces_vec.size(); iface++) // face_seq->total; iface++) { { - one_face.status = "good"; one_face.box2d = faces_vec[iface]; - one_face.id = i; // The cascade that computed this face. + one_face.id = i; // The cascade that computed this face. // Get the median disparity in the middle half of the bounding box. - cv::Mat disp_roi_shallow(*disparity_image_, cv::Rect(floor(one_face.box2d.x + 0.25 * one_face.box2d.width), - floor(one_face.box2d.y + 0.25 * one_face.box2d.height), - floor(one_face.box2d.x + 0.75 * one_face.box2d.width) - floor(one_face.box2d.x + 0.25 * one_face.box2d.width) + 1, - floor(one_face.box2d.y + 0.75 * one_face.box2d.height) - floor(one_face.box2d.y + 0.25 * one_face.box2d.height) + 1)); + cv::Mat disp_roi_shallow(*disparity_image_, + cv::Rect(floor(one_face.box2d.x + 0.25 * one_face.box2d.width), + floor(one_face.box2d.y + 0.25 * one_face.box2d.height), + floor(one_face.box2d.x + 0.75 * one_face.box2d.width) - + floor(one_face.box2d.x + 0.25 * one_face.box2d.width) + 1, + floor(one_face.box2d.y + 0.75 * one_face.box2d.height) - + floor(one_face.box2d.y + 0.25 * one_face.box2d.height) + 1)); cv::Mat disp_roi = disp_roi_shallow.clone(); cv::Mat tmat = disp_roi.reshape(1, disp_roi.rows * disp_roi.cols); cv::Mat tmat_sorted; cv::sort(tmat, tmat_sorted, CV_SORT_EVERY_COLUMN + CV_SORT_DESCENDING); - avg_disp = tmat_sorted.at(floor(cv::countNonZero(tmat_sorted >= 0.0) / 2.0)); // Get the middle valid disparity (-1 disparities are invalid) + + // Get the middle valid disparity (-1 disparities are invalid) + avg_disp = tmat_sorted.at(floor(cv::countNonZero(tmat_sorted >= 0.0) / 2.0)); // Fill in the rest of the face data structure. one_face.center2d = cv::Point2d(one_face.box2d.x + one_face.box2d.width / 2.0, @@ -217,7 +221,9 @@ void Faces::faceDetectionThreadDisparity(uint i) cam_model_->projectDisparityTo3d(cv::Point2d(one_face.box2d.width, 0.0), avg_disp, p3_2); one_face.width3d = fabs(p3_2.x - p3_1.x); cam_model_->projectDisparityTo3d(one_face.center2d, avg_disp, one_face.center3d); - if (one_face.center3d.z > max_face_z_m_ || one_face.width3d < face_size_min_m_ || one_face.width3d > face_size_max_m_) + if (one_face.center3d.z > max_face_z_m_ || + one_face.width3d < face_size_min_m_ || + one_face.width3d > face_size_max_m_) { one_face.status = "bad"; } @@ -229,7 +235,7 @@ void Faces::faceDetectionThreadDisparity(uint i) one_face.status = "unknown"; } - // Add faces to the output vector. + // Add faces to the output . // lock faces boost::mutex::scoped_lock lock(face_mutex_); faces_.push_back(one_face); @@ -244,10 +250,9 @@ void Faces::faceDetectionThreadDisparity(uint i) } ///// - -void Faces::initFaceDetectionDepth(uint num_cascades, string haar_classifier_filename, double face_size_min_m, double face_size_max_m, double max_face_z_m, double face_sep_dist_m) +void Faces::initFaceDetectionDepth(uint num_cascades, std::string haar_classifier_filename, double face_size_min_m, + double face_size_max_m, double max_face_z_m, double face_sep_dist_m) { - images_ready_ = 0; face_size_min_m_ = face_size_min_m; @@ -266,12 +271,10 @@ void Faces::initFaceDetectionDepth(uint num_cascades, string haar_classifier_fil threads_.create_thread(boost::bind(&Faces::faceDetectionThreadDepth, this, 0)); } - ///// - -vector Faces::detectAllFacesDepth(const cv::Mat &image, double threshold, const cv::Mat &dimage, image_geometry::StereoCameraModel *cam_model) +std::vector Faces::detectAllFacesDepth(const cv::Mat &image, double threshold, const cv::Mat &dimage, + image_geometry::StereoCameraModel *cam_model) { - faces_.clear(); // Convert the image to grayscale, if necessary. @@ -313,15 +316,13 @@ vector Faces::detectAllFacesDepth(const cv::Mat &image, double threshol } ///// - void Faces::faceDetectionThreadDepth(uint i) { - - while (1) + while (true) { boost::mutex::scoped_lock fgmlock(*(face_go_mutex_)); boost::mutex::scoped_lock tlock(t_mutex_, boost::defer_lock); - while (1) + while (true) { tlock.lock(); if (images_ready_) @@ -339,12 +340,14 @@ void Faces::faceDetectionThreadDepth(uint i) cv::Point3d p3_2(face_size_min_m_, 0, max_face_z_m_); cv::Point2d p2_1 = (cam_model_->left()).project3dToPixel(p3_1); cv::Point2d p2_2 = (cam_model_->left()).project3dToPixel(p3_2); - int this_min_face_size = (int)(floor(fabs(p2_2.x - p2_1.x))); + int this_min_face_size = static_cast(floor(fabs(p2_2.x - p2_1.x))); std::vector faces_vec; - cascade_.detectMultiScale(cv_image_gray_, faces_vec, 1.2, 2, CV_HAAR_DO_CANNY_PRUNING, cv::Size(this_min_face_size, this_min_face_size)); + cascade_.detectMultiScale(cv_image_gray_, faces_vec, 1.2, 2, CV_HAAR_DO_CANNY_PRUNING, + cv::Size(this_min_face_size, this_min_face_size)); - // Filter the faces using depth information, if available. Currently checks that the actual face size is within the given limits. + // Filter the faces using depth information, if available. + // Currently checks that the actual face size is within the given limits. cv::Scalar color(0, 255, 0); Box2D3D one_face; double avg_d = 0.0; @@ -353,24 +356,25 @@ void Faces::faceDetectionThreadDepth(uint i) // For each face... for (uint iface = 0; iface < faces_vec.size(); iface++) { - one_face.status = "good"; one_face.box2d = faces_vec[iface]; - one_face.id = i; // The cascade that computed this face. + one_face.id = i; // The cascade that computed this face. // Get the median depth in the middle half of the bounding box. - cv::Mat depth_roi_shallow(*depth_image_, cv::Rect(floor(one_face.box2d.x + 0.25 * one_face.box2d.width), - floor(one_face.box2d.y + 0.25 * one_face.box2d.height), - floor(one_face.box2d.x + 0.75 * one_face.box2d.width) - floor(one_face.box2d.x + 0.25 * one_face.box2d.width) + 1, - floor(one_face.box2d.y + 0.75 * one_face.box2d.height) - floor(one_face.box2d.y + 0.25 * one_face.box2d.height) + 1)); + cv::Mat depth_roi_shallow(*depth_image_, + cv::Rect(floor(one_face.box2d.x + 0.25 * one_face.box2d.width), + floor(one_face.box2d.y + 0.25 * one_face.box2d.height), + floor(one_face.box2d.x + 0.75 * one_face.box2d.width) - + floor(one_face.box2d.x + 0.25 * one_face.box2d.width) + 1, + floor(one_face.box2d.y + 0.75 * one_face.box2d.height) - + floor(one_face.box2d.y + 0.25 * one_face.box2d.height) + 1)); // Fill in the rest of the face data structure. one_face.center2d = cv::Point2d(one_face.box2d.x + one_face.box2d.width / 2.0, one_face.box2d.y + one_face.box2d.height / 2.0); one_face.width2d = one_face.box2d.width; - // If the median depth was valid and the face is a reasonable size, the face status is "good". // If the median depth was valid but the face isn't a reasonable size, the face status is "bad". // Otherwise, the face status is "unknown". @@ -401,7 +405,9 @@ void Faces::faceDetectionThreadDepth(uint i) one_face.center3d = (cam_model_->left()).projectPixelTo3dRay(one_face.center2d); one_face.center3d = (avg_d / one_face.center3d.z) * one_face.center3d; - if (one_face.center3d.z > max_face_z_m_ || one_face.width3d < face_size_min_m_ || one_face.width3d > face_size_max_m_) + if (one_face.center3d.z > max_face_z_m_ || + one_face.width3d < face_size_min_m_ || + one_face.width3d > face_size_max_m_) { one_face.status = "bad"; } @@ -425,7 +431,5 @@ void Faces::faceDetectionThreadDepth(uint i) fdmlock.unlock(); face_detection_done_cond_.notify_one(); } - } - -}; +}; // namespace people diff --git a/leg_detector/CMakeLists.txt b/leg_detector/CMakeLists.txt index 36293387..941c2e3a 100644 --- a/leg_detector/CMakeLists.txt +++ b/leg_detector/CMakeLists.txt @@ -4,28 +4,25 @@ project(leg_detector) # look for bfl (Bayesian Filtering Library) find_package(PkgConfig REQUIRED) pkg_check_modules(BFL REQUIRED orocos-bfl) - -include_directories(${BFL_INCLUDE_DIRS}) link_directories(${BFL_LIBRARY_DIRS}) # Look for Bullet pkg_check_modules(BULLET bullet) -include_directories(${BULLET_INCLUDE_DIRS}) find_package(catkin REQUIRED COMPONENTS - dynamic_reconfigure - geometry_msgs - image_geometry - laser_geometry - message_filters - people_msgs - people_tracking_filter - roscpp - sensor_msgs - std_msgs - std_srvs - tf - visualization_msgs + dynamic_reconfigure + geometry_msgs + image_geometry + laser_geometry + message_filters + people_msgs + people_tracking_filter + roscpp + sensor_msgs + std_msgs + std_srvs + tf + visualization_msgs ) ## dynamic reconfigure config @@ -33,10 +30,6 @@ generate_dynamic_reconfigure_options( cfg/LegDetector.cfg ) -include_directories( - include ${catkin_INCLUDE_DIRS} -) - catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS @@ -52,19 +45,30 @@ catkin_package( visualization_msgs ) +include_directories( + include ${catkin_INCLUDE_DIRS} ${BFL_INCLUDE_DIRS} ${BULLET_INCLUDE_DIRS} +) + add_executable(leg_detector src/laser_processor.cpp src/leg_detector.cpp src/calc_leg_features.cpp) -add_dependencies(leg_detector ${${PROJECT_NAME}_EXPORTED_TARGETS}) +add_dependencies(leg_detector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(leg_detector ${catkin_LIBRARIES} ${BFL_LIBRARIES} ${BULLET_LIBRARIES} ) +if(CATKIN_ENABLE_TESTING) + find_package(catkin REQUIRED COMPONENTS roslaunch roslint) + roslaunch_add_file_check(launch) + roslint_cpp() + roslint_add_test() +endif() + install(TARGETS leg_detector - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(FILES config/trained_leg_detector.yaml @@ -74,3 +78,7 @@ install(FILES config/trained_leg_detector.yaml install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) diff --git a/leg_detector/cfg/LegDetector.cfg b/leg_detector/cfg/LegDetector.cfg index 68b7ddc8..633454dd 100755 --- a/leg_detector/cfg/LegDetector.cfg +++ b/leg_detector/cfg/LegDetector.cfg @@ -1,7 +1,6 @@ #!/usr/bin/env python -PACKAGE = 'leg_detector' -from dynamic_reconfigure.parameter_generator_catkin import * +from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t, str_t gen = ParameterGenerator() @@ -27,4 +26,4 @@ gen.add('kalman_q', double_t, 0, '', .002, 0, 10) gen.add('kalman_r', double_t, 0, '', 10, 0, 20) gen.add('kalman_on', int_t, 0, '', 1, 0, 1) -exit(gen.generate(PACKAGE, 'leg_detector', 'LegDetector')) +exit(gen.generate('leg_detector', 'leg_detector', 'LegDetector')) diff --git a/leg_detector/launch/filtered_leg_detector.launch b/leg_detector/launch/filtered_leg_detector.launch index 607d20d5..6ad9a646 100644 --- a/leg_detector/launch/filtered_leg_detector.launch +++ b/leg_detector/launch/filtered_leg_detector.launch @@ -1,4 +1,5 @@ - + diff --git a/leg_detector/launch/leg_detector.launch b/leg_detector/launch/leg_detector.launch index 70b236ec..d9cabea9 100644 --- a/leg_detector/launch/leg_detector.launch +++ b/leg_detector/launch/leg_detector.launch @@ -1,4 +1,4 @@ - + - diff --git a/leg_detector/launch/shadows.launch b/leg_detector/launch/shadows.launch index 0a40fa48..3b15837f 100644 --- a/leg_detector/launch/shadows.launch +++ b/leg_detector/launch/shadows.launch @@ -4,5 +4,6 @@ - + diff --git a/leg_detector/package.xml b/leg_detector/package.xml index b7529647..9d726e9b 100644 --- a/leg_detector/package.xml +++ b/leg_detector/package.xml @@ -24,5 +24,7 @@ visualization_msgs laser_filters map_laser + roslaunch + roslint diff --git a/leg_detector/src/laser_processor.cpp b/leg_detector/src/laser_processor.cpp index 4478db3d..3dd22282 100644 --- a/leg_detector/src/laser_processor.cpp +++ b/leg_detector/src/laser_processor.cpp @@ -35,10 +35,11 @@ #include #include +#include +#include -using namespace ros; -using namespace std; -using namespace laser_processor; +namespace laser_processor +{ Sample* Sample::Extract(int ind, const sensor_msgs::LaserScan& scan) { @@ -63,7 +64,7 @@ void SampleSet::clear() i != end(); i++) { - delete(*i); + delete *i; } set::clear(); } @@ -73,7 +74,7 @@ void SampleSet::appendToCloud(sensor_msgs::PointCloud& cloud, int r, int g, int float color_val = 0; int rgb = (r << 16) | (g << 8) | b; - color_val = *(float*) & (rgb); + color_val = *(float*) & (rgb); // NOLINT(readability/casting) for (iterator sample_iter = begin(); sample_iter != end(); @@ -136,7 +137,7 @@ void ScanMask::addScan(sensor_msgs::LaserScan& scan) { if ((*m)->range > s->range) { - delete(*m); + delete *m; mask_.erase(m); mask_.insert(s); } @@ -192,26 +193,25 @@ ScanProcessor::ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_ } clusters_.push_back(cluster); - } ScanProcessor::~ScanProcessor() { - for (list::iterator c = clusters_.begin(); + for (std::list::iterator c = clusters_.begin(); c != clusters_.end(); c++) - delete(*c); + delete *c; } void ScanProcessor::removeLessThan(uint32_t num) { - list::iterator c_iter = clusters_.begin(); + std::list::iterator c_iter = clusters_.begin(); while (c_iter != clusters_.end()) { if ((*c_iter)->size() < num) { - delete(*c_iter); + delete *c_iter; clusters_.erase(c_iter++); } else @@ -225,9 +225,9 @@ ScanProcessor::removeLessThan(uint32_t num) void ScanProcessor::splitConnected(float thresh) { - list tmp_clusters; + std::list tmp_clusters; - list::iterator c_iter = clusters_.begin(); + std::list::iterator c_iter = clusters_.begin(); // For each cluster while (c_iter != clusters_.end()) @@ -239,16 +239,16 @@ ScanProcessor::splitConnected(float thresh) SampleSet::iterator s_first = (*c_iter)->begin(); // Start a new queue - list sample_queue; + std::list sample_queue; sample_queue.push_back(*s_first); (*c_iter)->erase(s_first); // Grow until we get to the end of the queue - list::iterator s_q = sample_queue.begin(); + std::list::iterator s_q = sample_queue.begin(); while (s_q != sample_queue.end()) { - int expand = (int)(asin(thresh / (*s_q)->range) / std::abs(scan_.angle_increment)); + int expand = static_cast(asin(thresh / (*s_q)->range) / std::abs(scan_.angle_increment)); SampleSet::iterator s_rest = (*c_iter)->begin(); @@ -283,7 +283,7 @@ ScanProcessor::splitConnected(float thresh) } // Now that c_iter is empty, we can delete - delete(*c_iter); + delete *c_iter; // And remove from the map clusters_.erase(c_iter++); @@ -291,3 +291,5 @@ ScanProcessor::splitConnected(float thresh) clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end()); } + +} // namespace laser_processor diff --git a/leg_detector/src/leg_detector.cpp b/leg_detector/src/leg_detector.cpp index c3c2c023..e5e457d2 100644 --- a/leg_detector/src/leg_detector.cpp +++ b/leg_detector/src/leg_detector.cpp @@ -61,14 +61,6 @@ #include #include -using namespace laser_processor; -using namespace ros; -using namespace tf; -using namespace estimation; -using namespace BFL; -using namespace MatrixWrapper; - - static double no_observation_timeout_s = 0.5; static double max_second_leg_age_s = 2.0; static double max_track_jump_m = 1.0; @@ -84,10 +76,10 @@ class SavedFeature { public: static int nextid; - TransformListener& tfl_; + tf::TransformListener& tfl_; BFL::StatePosVel sys_sigma_; - TrackerKalman filter_; + estimation::TrackerKalman filter_; std::string id_; std::string object_id; @@ -96,19 +88,19 @@ class SavedFeature double reliability, p; - Stamped position_; + tf::Stamped position_; SavedFeature* other; float dist_to_person_; // one leg tracker - SavedFeature(Stamped loc, TransformListener& tfl) + SavedFeature(tf::Stamped loc, tf::TransformListener& tfl) : tfl_(tfl), - sys_sigma_(Vector3(0.05, 0.05, 0.05), Vector3(1.0, 1.0, 1.0)), + sys_sigma_(tf::Vector3(0.05, 0.05, 0.05), tf::Vector3(1.0, 1.0, 1.0)), filter_("tracker_name", sys_sigma_), reliability(-1.), p(4) { char id[100]; - snprintf(id, 100, "legtrack%d", nextid++); + snprintf(id, sizeof(id), "legtrack%d", nextid++); id_ = std::string(id); object_id = ""; @@ -124,13 +116,13 @@ class SavedFeature { ROS_WARN("TF exception spot 6."); } - StampedTransform pose(Pose(Quaternion(0.0, 0.0, 0.0, 1.0), loc), loc.stamp_, id_, loc.frame_id_); + tf::StampedTransform pose(tf::Pose(tf::Quaternion(0.0, 0.0, 0.0, 1.0), loc), loc.stamp_, id_, loc.frame_id_); tfl_.setTransform(pose); - StatePosVel prior_sigma(Vector3(0.1, 0.1, 0.1), Vector3(0.0000001, 0.0000001, 0.0000001)); + BFL::StatePosVel prior_sigma(tf::Vector3(0.1, 0.1, 0.1), tf::Vector3(0.0000001, 0.0000001, 0.0000001)); filter_.initialize(loc, prior_sigma, time_.toSec()); - StatePosVel est; + BFL::StatePosVel est; filter_.getEstimate(est); updatePosition(); @@ -145,15 +137,15 @@ class SavedFeature updatePosition(); } - void update(Stamped loc, double probability) + void update(tf::Stamped loc, double probability) { - StampedTransform pose(Pose(Quaternion(0.0, 0.0, 0.0, 1.0), loc), loc.stamp_, id_, loc.frame_id_); + tf::StampedTransform pose(tf::Pose(tf::Quaternion(0.0, 0.0, 0.0, 1.0), loc), loc.stamp_, id_, loc.frame_id_); tfl_.setTransform(pose); meas_time_ = loc.stamp_; time_ = meas_time_; - SymmetricMatrix cov(3); + MatrixWrapper::SymmetricMatrix cov(3); cov = 0.0; cov(1, 1) = 0.0025; cov(2, 2) = 0.0025; @@ -190,7 +182,7 @@ class SavedFeature private: void updatePosition() { - StatePosVel est; + BFL::StatePosVel est; filter_.getEstimate(est); position_[0] = est.pos_[0]; @@ -211,12 +203,12 @@ int SavedFeature::nextid = 0; class MatchedFeature { public: - SampleSet* candidate_; + laser_processor::SampleSet* candidate_; SavedFeature* closest_; float distance_; double probability_; - MatchedFeature(SampleSet* candidate, SavedFeature* closest, float distance, double probability) + MatchedFeature(laser_processor::SampleSet* candidate, SavedFeature* closest, float distance, double probability) : candidate_(candidate) , closest_(closest) , distance_(distance) @@ -239,11 +231,11 @@ char** g_argv; class LegDetector { public: - NodeHandle nh_; + ros::NodeHandle nh_; - TransformListener tfl_; + tf::TransformListener tfl_; - ScanMask mask_; + laser_processor::ScanMask mask_; int mask_count_; @@ -278,7 +270,7 @@ class LegDetector tf::MessageFilter people_notifier_; tf::MessageFilter laser_notifier_; - LegDetector(ros::NodeHandle nh) : + explicit LegDetector(ros::NodeHandle nh) : nh_(nh), mask_count_(0), feat_count_(0), @@ -299,7 +291,7 @@ class LegDetector else { printf("Please provide a trained random forests classifier as an input.\n"); - shutdown(); + ros::shutdown(); } nh_.param("use_seeds", use_seeds_, !true); @@ -359,7 +351,7 @@ class LegDetector double distance(std::list::iterator it1, std::list::iterator it2) { - Stamped one = (*it1)->position_, two = (*it2)->position_; + tf::Stamped one = (*it1)->position_, two = (*it2)->position_; double dx = one[0] - two[0], dy = one[1] - two[1], dz = one[2] - two[2]; return sqrt(dx * dx + dy * dy + dz * dz); } @@ -373,13 +365,13 @@ class LegDetector if (saved_features_.empty()) return; - Point pt; + tf::Point pt; pointMsgToTF(people_meas->pos, pt); - Stamped person_loc(pt, people_meas->header.stamp, people_meas->header.frame_id); + tf::Stamped person_loc(pt, people_meas->header.stamp, people_meas->header.frame_id); person_loc[2] = 0.0; // Ignore the height of the person measurement. // Holder for all transformed pts. - Stamped dest_loc(pt, people_meas->header.stamp, people_meas->header.frame_id); + tf::Stamped dest_loc(pt, people_meas->header.stamp, people_meas->header.frame_id); boost::mutex::scoped_lock lock(saved_mutex_); @@ -419,7 +411,7 @@ class LegDetector } // Try to find one or two trackers with the same label and within the max distance of the person. - cout << "Looking for two legs" << endl; + std::cout << "Looking for two legs" << std::endl; it2 = end; for (it1 = begin; it1 != end; ++it1) { @@ -446,7 +438,7 @@ class LegDetector // If we found two legs with the right label and within the max distance, all is good, return. if (it1 != end && it2 != end) { - cout << "Found matching pair. The second distance was " << (*it1)->dist_to_person_ << endl; + std::cout << "Found matching pair. The second distance was " << (*it1)->dist_to_person_ << std::endl; return; } @@ -456,7 +448,7 @@ class LegDetector // * doesn't yet have a label (=valid precondition), // * is within the max distance, // * is less than max_second_leg_age_s old. - cout << "Looking for one leg plus one new leg" << endl; + std::cout << "Looking for one leg plus one new leg" << std::endl; float dist_between_legs, closest_dist_between_legs; if (it2 != end) { @@ -496,20 +488,20 @@ class LegDetector // If we found a close, unlabeled leg, set it's label. if (closest != end) { - cout << "Replaced one leg with a distance of " << closest_dist - << " and a distance between the legs of " << closest_dist_between_legs << endl; + std::cout << "Replaced one leg with a distance of " << closest_dist + << " and a distance between the legs of " << closest_dist_between_legs << std::endl; (*closest)->object_id = people_meas->object_id; } else { - cout << "Returned one matched leg only" << endl; + std::cout << "Returned one matched leg only" << std::endl; } // Regardless of whether we found a second leg, return. return; } - cout << "Looking for a pair of new legs" << endl; + std::cout << "Looking for a pair of new legs" << std::endl; // If we didn't find any legs with this person's label, // try to find two unlabeled legs that are close together and close to the tracker. it1 = saved_features_.begin(); @@ -569,21 +561,21 @@ class LegDetector { (*closest1)->object_id = people_meas->object_id; (*closest2)->object_id = people_meas->object_id; - cout << "Found a completely new pair with total distance " << closest_pair_dist - << " and a distance between the legs of " << closest_dist_between_legs << endl; + std::cout << "Found a completely new pair with total distance " << closest_pair_dist + << " and a distance between the legs of " << closest_dist_between_legs << std::endl; return; } - cout << "Looking for just one leg" << endl; + std::cout << "Looking for just one leg" << std::endl; // No pair worked, try for just one leg. if (closest != end) { (*closest)->object_id = people_meas->object_id; - cout << "Returned one new leg only" << endl; + std::cout << "Returned one new leg only" << std::endl; return; } - cout << "Nothing matched" << endl; + std::cout << "Nothing matched" << std::endl; } void pairLegs() @@ -678,7 +670,7 @@ class LegDetector if (best1 != end) { char id[100]; - snprintf(id, 100, "Person%d", next_p_id_++); + snprintf(id, sizeof(id), "Person%d", next_p_id_++); (*best1)->object_id = std::string(id); (*best2)->object_id = std::string(id); (*best1)->other = *best2; @@ -693,7 +685,7 @@ class LegDetector void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) { - ScanProcessor processor(*scan, mask_); + laser_processor::ScanProcessor processor(*scan, mask_); processor.splitConnected(connected_thresh_); processor.removeLessThan(5); @@ -709,7 +701,7 @@ class LegDetector { if ((*sf_iter)->other) (*sf_iter)->other->other = NULL; - delete(*sf_iter); + delete *sf_iter; saved_features_.erase(sf_iter++); } else @@ -732,7 +724,7 @@ class LegDetector // For each candidate, find the closest tracker (within threshold) and add to the match list // If no tracker is found, start a new one std::multiset matches; - for (std::list::iterator i = processor.getClusters().begin(); + for (std::list::iterator i = processor.getClusters().begin(); i != processor.getClusters().end(); i++) { @@ -741,10 +733,10 @@ class LegDetector memcpy(tmp_mat.data, f.data(), f.size()*sizeof(float)); float probability = 0.5 - - forest->predict(tmp_mat, cv::noArray(), cv::ml::RTrees::PREDICT_SUM) / - forest->getRoots().size(); + forest->predict(tmp_mat, cv::noArray(), cv::ml::RTrees::PREDICT_SUM) / + forest->getRoots().size(); - Stamped loc((*i)->center(), scan->header.stamp, scan->header.frame_id); + tf::Stamped loc((*i)->center(), scan->header.stamp, scan->header.frame_id); try { tfl_.transformPoint(fixed_frame, loc, loc); @@ -793,7 +785,7 @@ class LegDetector if (matched_iter->closest_ == *pf_iter) { // Transform candidate to fixed frame - Stamped loc(matched_iter->candidate_->center(), scan->header.stamp, scan->header.frame_id); + tf::Stamped loc(matched_iter->candidate_->center(), scan->header.stamp, scan->header.frame_id); try { tfl_.transformPoint(fixed_frame, loc, loc); @@ -823,7 +815,7 @@ class LegDetector // try to assign the candidate to another tracker if (!found) { - Stamped loc(matched_iter->candidate_->center(), scan->header.stamp, scan->header.frame_id); + tf::Stamped loc(matched_iter->candidate_->center(), scan->header.stamp, scan->header.frame_id); try { tfl_.transformPoint(fixed_frame, loc, loc); @@ -1012,4 +1004,3 @@ int main(int argc, char **argv) return 0; } - diff --git a/leg_detector/src/train_leg_detector.cpp b/leg_detector/src/train_leg_detector.cpp index 8c2b6eaa..750a387a 100644 --- a/leg_detector/src/train_leg_detector.cpp +++ b/leg_detector/src/train_leg_detector.cpp @@ -42,21 +42,21 @@ #include #include -using namespace std; -using namespace laser_processor; -using namespace ros; +#include +#include +#include enum LoadType {LOADING_NONE, LOADING_POS, LOADING_NEG, LOADING_TEST}; class TrainLegDetector { public: - ScanMask mask_; + laser_processor::ScanMask mask_; int mask_count_; - vector< vector > pos_data_; - vector< vector > neg_data_; - vector< vector > test_data_; + std::vector< std::vector > pos_data_; + std::vector< std::vector > neg_data_; + std::vector< std::vector > test_data_; CvRTrees forest; @@ -96,28 +96,28 @@ class TrainLegDetector switch (load) { case LOADING_POS: - p.addHandler(string("*"), &TrainLegDetector::loadCb, this, &pos_data_); + p.addHandler(std::string("*"), &TrainLegDetector::loadCb, this, &pos_data_); break; case LOADING_NEG: - mask_count_ = 1000; // effectively disable masking - p.addHandler(string("*"), &TrainLegDetector::loadCb, this, &neg_data_); + mask_count_ = 1000; // effectively disable masking + p.addHandler(std::string("*"), &TrainLegDetector::loadCb, this, &neg_data_); break; case LOADING_TEST: - p.addHandler(string("*"), &TrainLegDetector::loadCb, this, &test_data_); + p.addHandler(std::string("*"), &TrainLegDetector::loadCb, this, &test_data_); break; default: break; } while (p.nextMsg()) - {} + {} } } } - void loadCb(string name, sensor_msgs::LaserScan* scan, ros::Time t, ros::Time t_no_use, void* n) + void loadCb(std::string name, sensor_msgs::LaserScan* scan, ros::Time t, ros::Time t_no_use, void* n) { - vector< vector >* data = (vector< vector >*)(n); + std::vector< std::vector >* data = (std::vector< std::vector >*)(n); if (mask_count_++ < 20) { @@ -125,11 +125,11 @@ class TrainLegDetector } else { - ScanProcessor processor(*scan, mask_); + laser_processor::ScanProcessor processor(*scan, mask_); processor.splitConnected(connected_thresh_); processor.removeLessThan(5); - for (list::iterator i = processor.getClusters().begin(); + for (std::list::iterator i = processor.getClusters().begin(); i != processor.getClusters().end(); i++) data->push_back(calcLegFeatures(*i, *scan)); @@ -146,11 +146,11 @@ class TrainLegDetector // Put positive data in opencv format. int j = 0; - for (vector< vector >::iterator i = pos_data_.begin(); + for (std::vector< std::vector >::iterator i = pos_data_.begin(); i != pos_data_.end(); i++) { - float* data_row = (float*)(cv_data->data.ptr + cv_data->step * j); + float* data_row = reinterpret_cast(cv_data->data.ptr + cv_data->step * j); for (int k = 0; k < feat_count_; k++) data_row[k] = (*i)[k]; @@ -159,11 +159,11 @@ class TrainLegDetector } // Put negative data in opencv format. - for (vector< vector >::iterator i = neg_data_.begin(); + for (std::vector< std::vector >::iterator i = neg_data_.begin(); i != neg_data_.end(); i++) { - float* data_row = (float*)(cv_data->data.ptr + cv_data->step * j); + float* data_row = reinterpret_cast(cv_data->data.ptr + cv_data->step * j); for (int k = 0; k < feat_count_; k++) data_row[k] = (*i)[k]; @@ -195,12 +195,12 @@ class TrainLegDetector int pos_right = 0; int pos_total = 0; - for (vector< vector >::iterator i = pos_data_.begin(); + for (std::vector< std::vector >::iterator i = pos_data_.begin(); i != pos_data_.end(); i++) { for (int k = 0; k < feat_count_; k++) - tmp_mat->data.fl[k] = (float)((*i)[k]); + tmp_mat->data.fl[k] = static_cast((*i)[k]); if (forest.predict(tmp_mat) > 0) pos_right++; pos_total++; @@ -208,12 +208,12 @@ class TrainLegDetector int neg_right = 0; int neg_total = 0; - for (vector< vector >::iterator i = neg_data_.begin(); + for (std::vector< std::vector >::iterator i = neg_data_.begin(); i != neg_data_.end(); i++) { for (int k = 0; k < feat_count_; k++) - tmp_mat->data.fl[k] = (float)((*i)[k]); + tmp_mat->data.fl[k] = static_cast((*i)[k]); if (forest.predict(tmp_mat) < 0) neg_right++; neg_total++; @@ -221,23 +221,22 @@ class TrainLegDetector int test_right = 0; int test_total = 0; - for (vector< vector >::iterator i = test_data_.begin(); + for (std::vector< std::vector >::iterator i = test_data_.begin(); i != test_data_.end(); i++) { for (int k = 0; k < feat_count_; k++) - tmp_mat->data.fl[k] = (float)((*i)[k]); + tmp_mat->data.fl[k] = static_cast((*i)[k]); if (forest.predict(tmp_mat) > 0) test_right++; test_total++; } - printf(" Pos train set: %d/%d %g\n", pos_right, pos_total, (float)(pos_right) / pos_total); - printf(" Neg train set: %d/%d %g\n", neg_right, neg_total, (float)(neg_right) / neg_total); - printf(" Test set: %d/%d %g\n", test_right, test_total, (float)(test_right) / test_total); + printf(" Pos train set: %d/%d %g\n", pos_right, pos_total, static_cast(pos_right) / pos_total); + printf(" Neg train set: %d/%d %g\n", neg_right, neg_total, static_cast(neg_right) / neg_total); + printf(" Test set: %d/%d %g\n", test_right, test_total, static_cast(test_right) / test_total); cvReleaseMat(&tmp_mat); - } void save(char* file) diff --git a/people/package.xml b/people/package.xml index 1ed5f9a2..b3836968 100644 --- a/people/package.xml +++ b/people/package.xml @@ -3,6 +3,7 @@ 1.2.0 The people stack holds algorithms for perceiving people from a number of sensors. Dan Lazewatsky + David V. Lu!! BSD diff --git a/people_msgs/CMakeLists.txt b/people_msgs/CMakeLists.txt index 6420ee97..a8fcd9c3 100644 --- a/people_msgs/CMakeLists.txt +++ b/people_msgs/CMakeLists.txt @@ -13,7 +13,7 @@ add_message_files( ) generate_messages( - DEPENDENCIES geometry_msgs std_msgs + DEPENDENCIES geometry_msgs std_msgs ) catkin_package( diff --git a/people_msgs/package.xml b/people_msgs/package.xml index 969a2cc7..c4cd353b 100644 --- a/people_msgs/package.xml +++ b/people_msgs/package.xml @@ -3,6 +3,7 @@ 1.2.0 Messages used by nodes in the people stack. Dan Lazewatsky + David V. Lu!! BSD diff --git a/people_tracking_filter/CMakeLists.txt b/people_tracking_filter/CMakeLists.txt index b163e99c..ea66dcf4 100644 --- a/people_tracking_filter/CMakeLists.txt +++ b/people_tracking_filter/CMakeLists.txt @@ -4,8 +4,6 @@ project(people_tracking_filter) # Look for bfl (Bayesian Filtering Library) find_package(PkgConfig) pkg_check_modules(BFL REQUIRED orocos-bfl) - -include_directories(${BFL_INCLUDE_DIRS}/bfl) link_directories(${BFL_LIBRARY_DIRS}) find_package(catkin REQUIRED COMPONENTS @@ -34,7 +32,7 @@ catkin_package( ) include_directories( - include ${catkin_INCLUDE_DIRS} + include ${catkin_INCLUDE_DIRS} ${BFL_INCLUDE_DIRS}/bfl ) add_library(people_tracking_filter @@ -51,31 +49,33 @@ add_library(people_tracking_filter src/tracker_kalman.cpp src/detector_particle.cpp ) +add_dependencies(people_tracking_filter ${catkin_EXPORTED_TARGETS}) +target_link_libraries(people_tracking_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${BFL_LIBRARIES}) add_executable(people_tracker src/people_tracking_node.cpp) -add_dependencies(people_tracker people_tracking_filter) +add_dependencies(people_tracker people_tracking_filter ${catkin_EXPORTED_TARGETS}) target_link_libraries(people_tracker people_tracking_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${BFL_LIBRARIES} ) -target_link_libraries(people_tracking_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${BFL_LIBRARIES}) +if(CATKIN_ENABLE_TESTING) + find_package(catkin REQUIRED COMPONENTS roslaunch roslint) + roslaunch_add_file_check(launch) + roslint_cpp() + roslint_add_test() +endif() -install(TARGETS ${PROJECT_NAME} +install(TARGETS people_tracking_filter ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} ) - install(TARGETS people_tracker - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) - install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) - install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/people_tracking_filter/include/people_tracking_filter/detector_particle.h b/people_tracking_filter/include/people_tracking_filter/detector_particle.h index a0cc3bf2..a08d9312 100644 --- a/people_tracking_filter/include/people_tracking_filter/detector_particle.h +++ b/people_tracking_filter/include/people_tracking_filter/detector_particle.h @@ -34,16 +34,16 @@ /* Author: Wim Meeussen */ -#ifndef __DETECTOR_PARTICLE__ -#define __DETECTOR_PARTICLE__ +#ifndef PEOPLE_TRACKING_FILTER_DETECTOR_PARTICLE_H +#define PEOPLE_TRACKING_FILTER_DETECTOR_PARTICLE_H -#include "tracker.h" +#include // bayesian filtering #include -#include "mcpdf_vector.h" -#include "measmodel_vector.h" -#include "sysmodel_vector.h" +#include +#include +#include // TF #include @@ -61,7 +61,7 @@ class DetectorParticle { public: /// constructor - DetectorParticle(unsigned int num_particles); + explicit DetectorParticle(unsigned int num_particles); /// destructor ~DetectorParticle(); @@ -100,7 +100,7 @@ class DetectorParticle private: // pdf / model / filter BFL::MCPdfVector prior_; - BFL::BootstrapFilter* filter_; + BFL::BootstrapFilter* filter_; BFL::SysModelVector sys_model_; BFL::MeasModelVector meas_model_; @@ -108,10 +108,6 @@ class DetectorParticle bool detector_initialized_; double filter_time_, quality_; unsigned int num_particles_; - - -}; // class - -}; // namespace - -#endif +}; // class +} // namespace estimation +#endif // PEOPLE_TRACKING_FILTER_DETECTOR_PARTICLE_H diff --git a/people_tracking_filter/include/people_tracking_filter/gaussian_pos_vel.h b/people_tracking_filter/include/people_tracking_filter/gaussian_pos_vel.h index 0bce2a73..5c449395 100644 --- a/people_tracking_filter/include/people_tracking_filter/gaussian_pos_vel.h +++ b/people_tracking_filter/include/people_tracking_filter/gaussian_pos_vel.h @@ -34,13 +34,13 @@ /* Author: Wim Meeussen */ - -#ifndef GAUSSIAN_POS_VEL_H -#define GAUSSIAN_POS_VEL_H +#ifndef PEOPLE_TRACKING_FILTER_GAUSSIAN_POS_VEL_H +#define PEOPLE_TRACKING_FILTER_GAUSSIAN_POS_VEL_H #include -#include "state_pos_vel.h" -#include "gaussian_vector.h" +#include +#include +#include namespace BFL { @@ -73,13 +73,12 @@ class GaussianPosVel: public Pdf // Redefinition of pure virtuals virtual Probability ProbabilityGet(const StatePosVel& input) const; - bool SampleFrom(vector >& list_samples, const int num_samples, int method = DEFAULT, void * args = NULL) const; + bool SampleFrom(vector >& list_samples, const int num_samples, int method = DEFAULT, + void * args = NULL) const; virtual bool SampleFrom(Sample& one_sample, int method = DEFAULT, void * args = NULL) const; virtual StatePosVel ExpectedValueGet() const; virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; - }; - -} // end namespace -#endif +} // end namespace BFL +#endif // PEOPLE_TRACKING_FILTER_GAUSSIAN_POS_VEL_H diff --git a/people_tracking_filter/include/people_tracking_filter/gaussian_vector.h b/people_tracking_filter/include/people_tracking_filter/gaussian_vector.h index a5a96a09..b2284efa 100644 --- a/people_tracking_filter/include/people_tracking_filter/gaussian_vector.h +++ b/people_tracking_filter/include/people_tracking_filter/gaussian_vector.h @@ -34,13 +34,12 @@ /* Author: Wim Meeussen */ -#ifndef GAUSSIAN_VECTOR_H -#define GAUSSIAN_VECTOR_H +#ifndef PEOPLE_TRACKING_FILTER_GAUSSIAN_VECTOR_H +#define PEOPLE_TRACKING_FILTER_GAUSSIAN_VECTOR_H #include #include - - +#include namespace BFL { @@ -67,7 +66,8 @@ class GaussianVector: public Pdf // Redefinition of pure virtuals virtual Probability ProbabilityGet(const tf::Vector3& input) const; - bool SampleFrom(vector >& list_samples, const int num_samples, int method = DEFAULT, void * args = NULL) const; + bool SampleFrom(vector >& list_samples, const int num_samples, int method = DEFAULT, + void * args = NULL) const; virtual bool SampleFrom(Sample& one_sample, int method = DEFAULT, void * args = NULL) const; virtual tf::Vector3 ExpectedValueGet() const; @@ -75,6 +75,5 @@ class GaussianVector: public Pdf virtual GaussianVector* Clone() const; }; - -} // end namespace -#endif +} // end namespace BFL +#endif // PEOPLE_TRACKING_FILTER_GAUSSIAN_VECTOR_H diff --git a/people_tracking_filter/include/people_tracking_filter/mcpdf_pos_vel.h b/people_tracking_filter/include/people_tracking_filter/mcpdf_pos_vel.h index b1c655b0..9d2bde0e 100644 --- a/people_tracking_filter/include/people_tracking_filter/mcpdf_pos_vel.h +++ b/people_tracking_filter/include/people_tracking_filter/mcpdf_pos_vel.h @@ -34,11 +34,11 @@ /* Author: Wim Meeussen */ -#ifndef MCPDF_POSVEL_H -#define MCPDF_POSVEL_H +#ifndef PEOPLE_TRACKING_FILTER_MCPDF_POS_VEL_H +#define PEOPLE_TRACKING_FILTER_MCPDF_POS_VEL_H #include -#include "state_pos_vel.h" +#include #include #include @@ -49,7 +49,7 @@ class MCPdfPosVel: public MCPdf { public: /// Constructor - MCPdfPosVel(unsigned int num_samples); + explicit MCPdfPosVel(unsigned int num_samples); /// Destructor virtual ~MCPdfPosVel(); @@ -69,11 +69,8 @@ class MCPdfPosVel: public MCPdf private: /// Get histogram from certain area - MatrixWrapper::Matrix getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step, bool pos_hist) const; - + MatrixWrapper::Matrix getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step, + bool pos_hist) const; }; - - - -} // end namespace -#endif +} // end namespace BFL +#endif // PEOPLE_TRACKING_FILTER_MCPDF_POS_VEL_H diff --git a/people_tracking_filter/include/people_tracking_filter/mcpdf_vector.h b/people_tracking_filter/include/people_tracking_filter/mcpdf_vector.h index ca023a0e..a4217dec 100644 --- a/people_tracking_filter/include/people_tracking_filter/mcpdf_vector.h +++ b/people_tracking_filter/include/people_tracking_filter/mcpdf_vector.h @@ -34,8 +34,8 @@ /* Author: Wim Meeussen */ -#ifndef MCPDF_VECTOR_H -#define MCPDF_VECTOR_H +#ifndef PEOPLE_TRACKING_FILTER_MCPDF_VECTOR_H +#define PEOPLE_TRACKING_FILTER_MCPDF_VECTOR_H #include #include @@ -48,7 +48,7 @@ class MCPdfVector: public MCPdf { public: /// Constructor - MCPdfVector(unsigned int num_samples); + explicit MCPdfVector(unsigned int num_samples); /// Destructor virtual ~MCPdfVector(); @@ -62,10 +62,6 @@ class MCPdfVector: public MCPdf virtual tf::Vector3 ExpectedValueGet() const; virtual WeightedSample SampleGet(unsigned int particle) const; virtual unsigned int numParticlesGet() const; - }; - - - -} // end namespace -#endif +} // end namespace BFL +#endif // PEOPLE_TRACKING_FILTER_MCPDF_VECTOR_H diff --git a/people_tracking_filter/include/people_tracking_filter/measmodel_pos.h b/people_tracking_filter/include/people_tracking_filter/measmodel_pos.h index f4545258..b24fa8b7 100644 --- a/people_tracking_filter/include/people_tracking_filter/measmodel_pos.h +++ b/people_tracking_filter/include/people_tracking_filter/measmodel_pos.h @@ -34,12 +34,12 @@ /* Author: Wim Meeussen */ -#ifndef MEASMODEL_POS_H -#define MEASMODEL_POS_H +#ifndef PEOPLE_TRACKING_FILTER_MEASMODEL_POS_H +#define PEOPLE_TRACKING_FILTER_MEASMODEL_POS_H -#include "state_pos_vel.h" -#include "tf/tf.h" -#include "gaussian_vector.h" +#include +#include +#include #include #include #include @@ -47,13 +47,12 @@ namespace BFL { - class MeasPdfPos : public BFL::ConditionalPdf { public: /// Constructor - MeasPdfPos(const tf::Vector3& sigma); + explicit MeasPdfPos(const tf::Vector3& sigma); /// Destructor virtual ~MeasPdfPos(); @@ -64,26 +63,19 @@ class MeasPdfPos // Redefining pure virtual methods virtual BFL::Probability ProbabilityGet(const tf::Vector3& input) const; virtual bool SampleFrom(BFL::Sample& one_sample, int method, void *args) const; // Not applicable - virtual tf::Vector3 ExpectedValueGet() const; // Not applicable - virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable - + virtual tf::Vector3 ExpectedValueGet() const; // Not applicable + virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable private: GaussianVector meas_noise_; - -}; // class - - - - - +}; // class class MeasModelPos : public BFL::MeasurementModel { public: /// constructor - MeasModelPos(const tf::Vector3& sigma) + explicit MeasModelPos(const tf::Vector3& sigma) : BFL::MeasurementModel(new MeasPdfPos(sigma)) {}; @@ -92,10 +84,6 @@ class MeasModelPos { delete MeasurementPdfGet(); }; - -}; // class - -} //namespace - - -#endif +}; // class +} // namespace BFL +#endif // PEOPLE_TRACKING_FILTER_MEASMODEL_POS_H diff --git a/people_tracking_filter/include/people_tracking_filter/measmodel_vector.h b/people_tracking_filter/include/people_tracking_filter/measmodel_vector.h index 15b0017d..58d1fd23 100644 --- a/people_tracking_filter/include/people_tracking_filter/measmodel_vector.h +++ b/people_tracking_filter/include/people_tracking_filter/measmodel_vector.h @@ -35,11 +35,11 @@ /* Author: Wim Meeussen */ -#ifndef MEASMODEL_VECTOR_H -#define MEASMODEL_VECTOR_H +#ifndef PEOPLE_TRACKING_FILTER_MEASMODEL_VECTOR_H +#define PEOPLE_TRACKING_FILTER_MEASMODEL_VECTOR_H -#include "tf/tf.h" -#include "gaussian_vector.h" +#include +#include #include #include #include @@ -47,13 +47,12 @@ namespace BFL { - class MeasPdfVector : public BFL::ConditionalPdf { public: /// Constructor - MeasPdfVector(const tf::Vector3& sigma); + explicit MeasPdfVector(const tf::Vector3& sigma); /// Destructor virtual ~MeasPdfVector(); @@ -64,26 +63,19 @@ class MeasPdfVector // Redefining pure virtual methods virtual BFL::Probability ProbabilityGet(const tf::Vector3& input) const; virtual bool SampleFrom(BFL::Sample& one_sample, int method, void *args) const; // Not applicable - virtual tf::Vector3 ExpectedValueGet() const; // Not applicable - virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable - + virtual tf::Vector3 ExpectedValueGet() const; // Not applicable + virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable private: GaussianVector meas_noise_; - -}; // class - - - - - +}; // class class MeasModelVector : public BFL::MeasurementModel { public: /// constructor - MeasModelVector(const tf::Vector3& sigma) + explicit MeasModelVector(const tf::Vector3& sigma) : BFL::MeasurementModel(new MeasPdfVector(sigma)) {}; @@ -92,10 +84,6 @@ class MeasModelVector { delete MeasurementPdfGet(); }; - -}; // class - -} //namespace - - -#endif +}; // class +} // namespace BFL +#endif // PEOPLE_TRACKING_FILTER_MEASMODEL_VECTOR_H diff --git a/people_tracking_filter/include/people_tracking_filter/people_tracking_node.h b/people_tracking_filter/include/people_tracking_filter/people_tracking_node.h index 1b1fb9c1..2af51034 100644 --- a/people_tracking_filter/include/people_tracking_filter/people_tracking_node.h +++ b/people_tracking_filter/include/people_tracking_filter/people_tracking_node.h @@ -34,10 +34,11 @@ /* Author: Wim Meeussen */ -#ifndef __PEOPLE_TRACKING_NODE__ -#define __PEOPLE_TRACKING_NODE__ +#ifndef PEOPLE_TRACKING_FILTER_PEOPLE_TRACKING_NODE_H +#define PEOPLE_TRACKING_FILTER_PEOPLE_TRACKING_NODE_H #include +#include #include // ros stuff @@ -46,9 +47,9 @@ #include // people tracking stuff -#include "tracker.h" -#include "detector_particle.h" -#include "gaussian_vector.h" +#include +#include +#include // messages #include @@ -67,7 +68,7 @@ class PeopleTrackingNode { public: /// constructor - PeopleTrackingNode(ros::NodeHandle nh); + explicit PeopleTrackingNode(ros::NodeHandle nh); /// destructor virtual ~PeopleTrackingNode(); @@ -81,9 +82,7 @@ class PeopleTrackingNode /// tracker loop void spin(); - private: - ros::NodeHandle nh_; ros::Publisher people_filter_pub_; @@ -112,10 +111,6 @@ class PeopleTrackingNode // Track only one person who the robot will follow. bool follow_one_person_; - - -}; // class - -}; // namespace - -#endif +}; // class +} // namespace estimation +#endif // PEOPLE_TRACKING_FILTER_PEOPLE_TRACKING_NODE_H diff --git a/people_tracking_filter/include/people_tracking_filter/rgb.h b/people_tracking_filter/include/people_tracking_filter/rgb.h index 2a4a0001..6dfbdf62 100644 --- a/people_tracking_filter/include/people_tracking_filter/rgb.h +++ b/people_tracking_filter/include/people_tracking_filter/rgb.h @@ -1,1008 +1,1040 @@ -#ifndef __RGB__ -#define __RGB__ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +#ifndef PEOPLE_TRACKING_FILTER_RGB_H +#define PEOPLE_TRACKING_FILTER_RGB_H -const static int rgb[] = +static const int rgb[] = { - (0x00 << 16) | (0x00 << 8) | (0x77 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x7f << 00) , - (0x00 << 16) | (0x00 << 8) | (0x80 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x82 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x84 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x84 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x84 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x85 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x89 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x89 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x89 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x89 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x8d << 00) , - (0x00 << 16) | (0x00 << 8) | (0x8d << 00) , - (0x00 << 16) | (0x00 << 8) | (0x8d << 00) , - (0x00 << 16) | (0x00 << 8) | (0x8e << 00) , - (0x00 << 16) | (0x00 << 8) | (0x92 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x92 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x92 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x92 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x96 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x96 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x96 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x98 << 00) , - (0x00 << 16) | (0x00 << 8) | (0x9b << 00) , - (0x00 << 16) | (0x00 << 8) | (0x9b << 00) , - (0x00 << 16) | (0x00 << 8) | (0x9b << 00) , - (0x00 << 16) | (0x00 << 8) | (0x9b << 00) , - (0x00 << 16) | (0x00 << 8) | (0x9f << 00) , - (0x00 << 16) | (0x00 << 8) | (0x9f << 00) , - (0x00 << 16) | (0x00 << 8) | (0x9f << 00) , - (0x00 << 16) | (0x00 << 8) | (0xa1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xa4 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xa4 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xa4 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xa4 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xa8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xa8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xa8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xa8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xad << 00) , - (0x00 << 16) | (0x00 << 8) | (0xad << 00) , - (0x00 << 16) | (0x00 << 8) | (0xad << 00) , - (0x00 << 16) | (0x00 << 8) | (0xb1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xb1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xb1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xb3 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xb6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xb6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xb6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xb6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xbb << 00) , - (0x00 << 16) | (0x00 << 8) | (0xbb << 00) , - (0x00 << 16) | (0x00 << 8) | (0xbb << 00) , - (0x00 << 16) | (0x00 << 8) | (0xbd << 00) , - (0x00 << 16) | (0x00 << 8) | (0xbf << 00) , - (0x00 << 16) | (0x00 << 8) | (0xbf << 00) , - (0x00 << 16) | (0x00 << 8) | (0xbf << 00) , - (0x00 << 16) | (0x00 << 8) | (0xbf << 00) , - (0x00 << 16) | (0x00 << 8) | (0xc4 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xc4 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xc4 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xc4 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xc8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xc8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xc8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xc9 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xcd << 00) , - (0x00 << 16) | (0x00 << 8) | (0xcd << 00) , - (0x00 << 16) | (0x00 << 8) | (0xcd << 00) , - (0x00 << 16) | (0x00 << 8) | (0xcd << 00) , - (0x00 << 16) | (0x00 << 8) | (0xd1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xd1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xd1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xd2 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xd6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xd6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xd6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xd6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xda << 00) , - (0x00 << 16) | (0x00 << 8) | (0xda << 00) , - (0x00 << 16) | (0x00 << 8) | (0xda << 00) , - (0x00 << 16) | (0x00 << 8) | (0xdf << 00) , - (0x00 << 16) | (0x00 << 8) | (0xdf << 00) , - (0x00 << 16) | (0x00 << 8) | (0xdf << 00) , - (0x00 << 16) | (0x00 << 8) | (0xdf << 00) , - (0x00 << 16) | (0x00 << 8) | (0xe3 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xe3 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xe3 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xe4 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xe8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xe8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xe8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xe8 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xed << 00) , - (0x00 << 16) | (0x00 << 8) | (0xed << 00) , - (0x00 << 16) | (0x00 << 8) | (0xed << 00) , - (0x00 << 16) | (0x00 << 8) | (0xee << 00) , - (0x00 << 16) | (0x00 << 8) | (0xf1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xf1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xf1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xf1 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xf6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xf6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xf6 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xf7 << 00) , - (0x00 << 16) | (0x00 << 8) | (0xfa << 00) , - (0x00 << 16) | (0x00 << 8) | (0xfa << 00) , - (0x00 << 16) | (0x00 << 8) | (0xfa << 00) , - (0x00 << 16) | (0x00 << 8) | (0xfa << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x00 << 8) | (0xff << 00) , - (0x00 << 16) | (0x01 << 8) | (0xff << 00) , - (0x00 << 16) | (0x04 << 8) | (0xff << 00) , - (0x00 << 16) | (0x04 << 8) | (0xff << 00) , - (0x00 << 16) | (0x04 << 8) | (0xff << 00) , - (0x00 << 16) | (0x04 << 8) | (0xff << 00) , - (0x00 << 16) | (0x08 << 8) | (0xff << 00) , - (0x00 << 16) | (0x08 << 8) | (0xff << 00) , - (0x00 << 16) | (0x08 << 8) | (0xff << 00) , - (0x00 << 16) | (0x0a << 8) | (0xff << 00) , - (0x00 << 16) | (0x0c << 8) | (0xff << 00) , - (0x00 << 16) | (0x0c << 8) | (0xff << 00) , - (0x00 << 16) | (0x0c << 8) | (0xff << 00) , - (0x00 << 16) | (0x0c << 8) | (0xff << 00) , - (0x00 << 16) | (0x11 << 8) | (0xff << 00) , - (0x00 << 16) | (0x11 << 8) | (0xff << 00) , - (0x00 << 16) | (0x11 << 8) | (0xff << 00) , - (0x00 << 16) | (0x11 << 8) | (0xff << 00) , - (0x00 << 16) | (0x15 << 8) | (0xff << 00) , - (0x00 << 16) | (0x15 << 8) | (0xff << 00) , - (0x00 << 16) | (0x15 << 8) | (0xff << 00) , - (0x00 << 16) | (0x15 << 8) | (0xff << 00) , - (0x00 << 16) | (0x19 << 8) | (0xff << 00) , - (0x00 << 16) | (0x19 << 8) | (0xff << 00) , - (0x00 << 16) | (0x19 << 8) | (0xff << 00) , - (0x00 << 16) | (0x19 << 8) | (0xff << 00) , - (0x00 << 16) | (0x1d << 8) | (0xff << 00) , - (0x00 << 16) | (0x1d << 8) | (0xff << 00) , - (0x00 << 16) | (0x1d << 8) | (0xff << 00) , - (0x00 << 16) | (0x1e << 8) | (0xff << 00) , - (0x00 << 16) | (0x20 << 8) | (0xff << 00) , - (0x00 << 16) | (0x20 << 8) | (0xff << 00) , - (0x00 << 16) | (0x22 << 8) | (0xff << 00) , - (0x00 << 16) | (0x24 << 8) | (0xff << 00) , - (0x00 << 16) | (0x24 << 8) | (0xff << 00) , - (0x00 << 16) | (0x24 << 8) | (0xff << 00) , - (0x00 << 16) | (0x24 << 8) | (0xff << 00) , - (0x00 << 16) | (0x28 << 8) | (0xff << 00) , - (0x00 << 16) | (0x28 << 8) | (0xff << 00) , - (0x00 << 16) | (0x28 << 8) | (0xff << 00) , - (0x00 << 16) | (0x28 << 8) | (0xff << 00) , - (0x00 << 16) | (0x2c << 8) | (0xff << 00) , - (0x00 << 16) | (0x2c << 8) | (0xff << 00) , - (0x00 << 16) | (0x2c << 8) | (0xff << 00) , - (0x00 << 16) | (0x2c << 8) | (0xff << 00) , - (0x00 << 16) | (0x30 << 8) | (0xff << 00) , - (0x00 << 16) | (0x30 << 8) | (0xff << 00) , - (0x00 << 16) | (0x30 << 8) | (0xff << 00) , - (0x00 << 16) | (0x30 << 8) | (0xff << 00) , - (0x00 << 16) | (0x34 << 8) | (0xff << 00) , - (0x00 << 16) | (0x34 << 8) | (0xff << 00) , - (0x00 << 16) | (0x34 << 8) | (0xff << 00) , - (0x00 << 16) | (0x35 << 8) | (0xff << 00) , - (0x00 << 16) | (0x38 << 8) | (0xff << 00) , - (0x00 << 16) | (0x38 << 8) | (0xff << 00) , - (0x00 << 16) | (0x38 << 8) | (0xff << 00) , - (0x00 << 16) | (0x38 << 8) | (0xff << 00) , - (0x00 << 16) | (0x3c << 8) | (0xff << 00) , - (0x00 << 16) | (0x3c << 8) | (0xff << 00) , - (0x00 << 16) | (0x3c << 8) | (0xff << 00) , - (0x00 << 16) | (0x3d << 8) | (0xff << 00) , - (0x00 << 16) | (0x40 << 8) | (0xff << 00) , - (0x00 << 16) | (0x40 << 8) | (0xff << 00) , - (0x00 << 16) | (0x40 << 8) | (0xff << 00) , - (0x00 << 16) | (0x40 << 8) | (0xff << 00) , - (0x00 << 16) | (0x44 << 8) | (0xff << 00) , - (0x00 << 16) | (0x44 << 8) | (0xff << 00) , - (0x00 << 16) | (0x44 << 8) | (0xff << 00) , - (0x00 << 16) | (0x44 << 8) | (0xff << 00) , - (0x00 << 16) | (0x47 << 8) | (0xff << 00) , - (0x00 << 16) | (0x48 << 8) | (0xff << 00) , - (0x00 << 16) | (0x48 << 8) | (0xff << 00) , - (0x00 << 16) | (0x48 << 8) | (0xff << 00) , - (0x00 << 16) | (0x4c << 8) | (0xff << 00) , - (0x00 << 16) | (0x4c << 8) | (0xff << 00) , - (0x00 << 16) | (0x4d << 8) | (0xff << 00) , - (0x00 << 16) | (0x50 << 8) | (0xff << 00) , - (0x00 << 16) | (0x50 << 8) | (0xff << 00) , - (0x00 << 16) | (0x50 << 8) | (0xff << 00) , - (0x00 << 16) | (0x50 << 8) | (0xff << 00) , - (0x00 << 16) | (0x54 << 8) | (0xff << 00) , - (0x00 << 16) | (0x54 << 8) | (0xff << 00) , - (0x00 << 16) | (0x54 << 8) | (0xff << 00) , - (0x00 << 16) | (0x55 << 8) | (0xff << 00) , - (0x00 << 16) | (0x58 << 8) | (0xff << 00) , - (0x00 << 16) | (0x58 << 8) | (0xff << 00) , - (0x00 << 16) | (0x58 << 8) | (0xff << 00) , - (0x00 << 16) | (0x58 << 8) | (0xff << 00) , - (0x00 << 16) | (0x5c << 8) | (0xff << 00) , - (0x00 << 16) | (0x5c << 8) | (0xff << 00) , - (0x00 << 16) | (0x5c << 8) | (0xff << 00) , - (0x00 << 16) | (0x5c << 8) | (0xff << 00) , - (0x00 << 16) | (0x5f << 8) | (0xff << 00) , - (0x00 << 16) | (0x60 << 8) | (0xff << 00) , - (0x00 << 16) | (0x60 << 8) | (0xff << 00) , - (0x00 << 16) | (0x60 << 8) | (0xff << 00) , - (0x00 << 16) | (0x64 << 8) | (0xff << 00) , - (0x00 << 16) | (0x64 << 8) | (0xff << 00) , - (0x00 << 16) | (0x64 << 8) | (0xff << 00) , - (0x00 << 16) | (0x64 << 8) | (0xff << 00) , - (0x00 << 16) | (0x68 << 8) | (0xff << 00) , - (0x00 << 16) | (0x68 << 8) | (0xff << 00) , - (0x00 << 16) | (0x68 << 8) | (0xff << 00) , - (0x00 << 16) | (0x68 << 8) | (0xff << 00) , - (0x00 << 16) | (0x6c << 8) | (0xff << 00) , - (0x00 << 16) | (0x6c << 8) | (0xff << 00) , - (0x00 << 16) | (0x6c << 8) | (0xff << 00) , - (0x00 << 16) | (0x6c << 8) | (0xff << 00) , - (0x00 << 16) | (0x70 << 8) | (0xff << 00) , - (0x00 << 16) | (0x70 << 8) | (0xff << 00) , - (0x00 << 16) | (0x70 << 8) | (0xff << 00) , - (0x00 << 16) | (0x71 << 8) | (0xff << 00) , - (0x00 << 16) | (0x74 << 8) | (0xff << 00) , - (0x00 << 16) | (0x74 << 8) | (0xff << 00) , - (0x00 << 16) | (0x74 << 8) | (0xff << 00) , - (0x00 << 16) | (0x77 << 8) | (0xff << 00) , - (0x00 << 16) | (0x78 << 8) | (0xff << 00) , - (0x00 << 16) | (0x78 << 8) | (0xff << 00) , - (0x00 << 16) | (0x78 << 8) | (0xff << 00) , - (0x00 << 16) | (0x7c << 8) | (0xff << 00) , - (0x00 << 16) | (0x7c << 8) | (0xff << 00) , - (0x00 << 16) | (0x7c << 8) | (0xff << 00) , - (0x00 << 16) | (0x7c << 8) | (0xff << 00) , - (0x00 << 16) | (0x80 << 8) | (0xff << 00) , - (0x00 << 16) | (0x80 << 8) | (0xff << 00) , - (0x00 << 16) | (0x80 << 8) | (0xff << 00) , - (0x00 << 16) | (0x80 << 8) | (0xff << 00) , - (0x00 << 16) | (0x85 << 8) | (0xff << 00) , - (0x00 << 16) | (0x85 << 8) | (0xff << 00) , - (0x00 << 16) | (0x85 << 8) | (0xff << 00) , - (0x00 << 16) | (0x85 << 8) | (0xff << 00) , - (0x00 << 16) | (0x88 << 8) | (0xff << 00) , - (0x00 << 16) | (0x88 << 8) | (0xff << 00) , - (0x00 << 16) | (0x88 << 8) | (0xff << 00) , - (0x00 << 16) | (0x89 << 8) | (0xff << 00) , - (0x00 << 16) | (0x8d << 8) | (0xff << 00) , - (0x00 << 16) | (0x8d << 8) | (0xff << 00) , - (0x00 << 16) | (0x8d << 8) | (0xff << 00) , - (0x00 << 16) | (0x8d << 8) | (0xff << 00) , - (0x00 << 16) | (0x90 << 8) | (0xff << 00) , - (0x00 << 16) | (0x90 << 8) | (0xff << 00) , - (0x00 << 16) | (0x90 << 8) | (0xff << 00) , - (0x00 << 16) | (0x92 << 8) | (0xff << 00) , - (0x00 << 16) | (0x95 << 8) | (0xff << 00) , - (0x00 << 16) | (0x95 << 8) | (0xff << 00) , - (0x00 << 16) | (0x95 << 8) | (0xff << 00) , - (0x00 << 16) | (0x95 << 8) | (0xff << 00) , - (0x00 << 16) | (0x98 << 8) | (0xff << 00) , - (0x00 << 16) | (0x98 << 8) | (0xff << 00) , - (0x00 << 16) | (0x98 << 8) | (0xff << 00) , - (0x00 << 16) | (0x98 << 8) | (0xff << 00) , - (0x00 << 16) | (0x9c << 8) | (0xff << 00) , - (0x00 << 16) | (0x9d << 8) | (0xff << 00) , - (0x00 << 16) | (0x9d << 8) | (0xff << 00) , - (0x00 << 16) | (0xa0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa1 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xa9 << 8) | (0xff << 00) , - (0x00 << 16) | (0xad << 8) | (0xff << 00) , - (0x00 << 16) | (0xad << 8) | (0xff << 00) , - (0x00 << 16) | (0xad << 8) | (0xff << 00) , - (0x00 << 16) | (0xad << 8) | (0xff << 00) , - (0x00 << 16) | (0xb0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb4 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xb8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xbd << 8) | (0xff << 00) , - (0x00 << 16) | (0xbd << 8) | (0xff << 00) , - (0x00 << 16) | (0xbd << 8) | (0xff << 00) , - (0x00 << 16) | (0xbd << 8) | (0xff << 00) , - (0x00 << 16) | (0xc0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xc0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xc0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xc0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xc5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xc5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xc5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xc8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xc8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xc8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xc8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xcc << 8) | (0xff << 00) , - (0x00 << 16) | (0xcd << 8) | (0xff << 00) , - (0x00 << 16) | (0xcd << 8) | (0xff << 00) , - (0x00 << 16) | (0xcd << 8) | (0xff << 00) , - (0x00 << 16) | (0xd0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd0 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd4 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd5 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xd8 << 8) | (0xff << 00) , - (0x00 << 16) | (0xdd << 8) | (0xfe << 00) , - (0x00 << 16) | (0xdd << 8) | (0xfe << 00) , - (0x00 << 16) | (0xdd << 8) | (0xfe << 00) , - (0x00 << 16) | (0xdd << 8) | (0xfd << 00) , - (0x00 << 16) | (0xe0 << 8) | (0xfb << 00) , - (0x00 << 16) | (0xe0 << 8) | (0xfb << 00) , - (0x00 << 16) | (0xe0 << 8) | (0xfb << 00) , - (0x00 << 16) | (0xe0 << 8) | (0xfb << 00) , - (0x00 << 16) | (0xe5 << 8) | (0xf8 << 00) , - (0x00 << 16) | (0xe5 << 8) | (0xf8 << 00) , - (0x00 << 16) | (0xe5 << 8) | (0xf8 << 00) , - (0x00 << 16) | (0xe5 << 8) | (0xf8 << 00) , - (0x01 << 16) | (0xe7 << 8) | (0xf4 << 00) , - (0x02 << 16) | (0xe8 << 8) | (0xf4 << 00) , - (0x02 << 16) | (0xe8 << 8) | (0xf4 << 00) , - (0x02 << 16) | (0xe8 << 8) | (0xf4 << 00) , - (0x06 << 16) | (0xed << 8) | (0xf1 << 00) , - (0x06 << 16) | (0xed << 8) | (0xf1 << 00) , - (0x06 << 16) | (0xed << 8) | (0xf1 << 00) , - (0x06 << 16) | (0xed << 8) | (0xf1 << 00) , - (0x08 << 16) | (0xef << 8) | (0xee << 00) , - (0x09 << 16) | (0xf0 << 8) | (0xee << 00) , - (0x09 << 16) | (0xf0 << 8) | (0xee << 00) , - (0x0c << 16) | (0xf5 << 8) | (0xeb << 00) , - (0x0c << 16) | (0xf5 << 8) | (0xeb << 00) , - (0x0c << 16) | (0xf5 << 8) | (0xeb << 00) , - (0x0c << 16) | (0xf5 << 8) | (0xea << 00) , - (0x0f << 16) | (0xf8 << 8) | (0xe7 << 00) , - (0x0f << 16) | (0xf8 << 8) | (0xe7 << 00) , - (0x0f << 16) | (0xf8 << 8) | (0xe7 << 00) , - (0x0f << 16) | (0xf8 << 8) | (0xe7 << 00) , - (0x13 << 16) | (0xfd << 8) | (0xe4 << 00) , - (0x13 << 16) | (0xfd << 8) | (0xe4 << 00) , - (0x13 << 16) | (0xfd << 8) | (0xe4 << 00) , - (0x13 << 16) | (0xfd << 8) | (0xe4 << 00) , - (0x15 << 16) | (0xfe << 8) | (0xe1 << 00) , - (0x00 << 16) | (0xff << 8) | (0xe1 << 00) , - (0x00 << 16) | (0xff << 8) | (0xe1 << 00) , - (0x00 << 16) | (0xff << 8) | (0xe1 << 00) , - (0x19 << 16) | (0xff << 8) | (0xde << 00) , - (0x19 << 16) | (0xff << 8) | (0xde << 00) , - (0x19 << 16) | (0xff << 8) | (0xde << 00) , - (0x19 << 16) | (0xff << 8) | (0xde << 00) , - (0x1b << 16) | (0xff << 8) | (0xdb << 00) , - (0x1c << 16) | (0xff << 8) | (0xdb << 00) , - (0x1c << 16) | (0xff << 8) | (0xdb << 00) , - (0x1c << 16) | (0xff << 8) | (0xda << 00) , - (0x1f << 16) | (0xff << 8) | (0xd7 << 00) , - (0x1f << 16) | (0xff << 8) | (0xd7 << 00) , - (0x1f << 16) | (0xff << 8) | (0xd7 << 00) , - (0x1f << 16) | (0xff << 8) | (0xd7 << 00) , - (0x22 << 16) | (0xff << 8) | (0xd4 << 00) , - (0x23 << 16) | (0xff << 8) | (0xd4 << 00) , - (0x23 << 16) | (0xff << 8) | (0xd4 << 00) , - (0x23 << 16) | (0xff << 8) | (0xd3 << 00) , - (0x26 << 16) | (0xff << 8) | (0xd1 << 00) , - (0x26 << 16) | (0xff << 8) | (0xd1 << 00) , - (0x26 << 16) | (0xff << 8) | (0xd1 << 00) , - (0x26 << 16) | (0xff << 8) | (0xd1 << 00) , - (0x29 << 16) | (0xff << 8) | (0xce << 00) , - (0x29 << 16) | (0xff << 8) | (0xce << 00) , - (0x29 << 16) | (0xff << 8) | (0xce << 00) , - (0x2c << 16) | (0xff << 8) | (0xca << 00) , - (0x2c << 16) | (0xff << 8) | (0xca << 00) , - (0x2c << 16) | (0xff << 8) | (0xca << 00) , - (0x2c << 16) | (0xff << 8) | (0xca << 00) , - (0x2f << 16) | (0xff << 8) | (0xc7 << 00) , - (0x30 << 16) | (0xff << 8) | (0xc7 << 00) , - (0x30 << 16) | (0xff << 8) | (0xc7 << 00) , - (0x30 << 16) | (0xff << 8) | (0xc6 << 00) , - (0x33 << 16) | (0xff << 8) | (0xc4 << 00) , - (0x33 << 16) | (0xff << 8) | (0xc4 << 00) , - (0x33 << 16) | (0xff << 8) | (0xc4 << 00) , - (0x33 << 16) | (0xff << 8) | (0xc4 << 00) , - (0x35 << 16) | (0xff << 8) | (0xc1 << 00) , - (0x36 << 16) | (0xff << 8) | (0xc1 << 00) , - (0x36 << 16) | (0xff << 8) | (0xc1 << 00) , - (0x36 << 16) | (0xff << 8) | (0xc0 << 00) , - (0x39 << 16) | (0xff << 8) | (0xbe << 00) , - (0x39 << 16) | (0xff << 8) | (0xbe << 00) , - (0x39 << 16) | (0xff << 8) | (0xbe << 00) , - (0x39 << 16) | (0xff << 8) | (0xbe << 00) , - (0x3c << 16) | (0xff << 8) | (0xba << 00) , - (0x3c << 16) | (0xff << 8) | (0xba << 00) , - (0x3c << 16) | (0xff << 8) | (0xba << 00) , - (0x3d << 16) | (0xff << 8) | (0xb9 << 00) , - (0x40 << 16) | (0xff << 8) | (0xb7 << 00) , - (0x40 << 16) | (0xff << 8) | (0xb7 << 00) , - (0x40 << 16) | (0xff << 8) | (0xb7 << 00) , - (0x40 << 16) | (0xff << 8) | (0xb7 << 00) , - (0x43 << 16) | (0xff << 8) | (0xb4 << 00) , - (0x43 << 16) | (0xff << 8) | (0xb4 << 00) , - (0x43 << 16) | (0xff << 8) | (0xb4 << 00) , - (0x43 << 16) | (0xff << 8) | (0xb4 << 00) , - (0x45 << 16) | (0xff << 8) | (0xb1 << 00) , - (0x46 << 16) | (0xff << 8) | (0xb1 << 00) , - (0x46 << 16) | (0xff << 8) | (0xb1 << 00) , - (0x46 << 16) | (0xff << 8) | (0xb1 << 00) , - (0x49 << 16) | (0xff << 8) | (0xad << 00) , - (0x49 << 16) | (0xff << 8) | (0xad << 00) , - (0x49 << 16) | (0xff << 8) | (0xac << 00) , - (0x4d << 16) | (0xff << 8) | (0xaa << 00) , - (0x4d << 16) | (0xff << 8) | (0xaa << 00) , - (0x4d << 16) | (0xff << 8) | (0xaa << 00) , - (0x4d << 16) | (0xff << 8) | (0xaa << 00) , - (0x50 << 16) | (0xff << 8) | (0xa7 << 00) , - (0x50 << 16) | (0xff << 8) | (0xa7 << 00) , - (0x50 << 16) | (0xff << 8) | (0xa7 << 00) , - (0x50 << 16) | (0xff << 8) | (0xa6 << 00) , - (0x53 << 16) | (0xff << 8) | (0xa4 << 00) , - (0x53 << 16) | (0xff << 8) | (0xa4 << 00) , - (0x53 << 16) | (0xff << 8) | (0xa4 << 00) , - (0x53 << 16) | (0xff << 8) | (0xa4 << 00) , - (0x56 << 16) | (0xff << 8) | (0xa0 << 00) , - (0x56 << 16) | (0xff << 8) | (0xa0 << 00) , - (0x56 << 16) | (0xff << 8) | (0xa0 << 00) , - (0x56 << 16) | (0xff << 8) | (0xa0 << 00) , - (0x58 << 16) | (0xff << 8) | (0x9d << 00) , - (0x5a << 16) | (0xff << 8) | (0x9d << 00) , - (0x5a << 16) | (0xff << 8) | (0x9d << 00) , - (0x5a << 16) | (0xff << 8) | (0x9d << 00) , - (0x5d << 16) | (0xff << 8) | (0x9a << 00) , - (0x5d << 16) | (0xff << 8) | (0x9a << 00) , - (0x5d << 16) | (0xff << 8) | (0x9a << 00) , - (0x5d << 16) | (0xff << 8) | (0x9a << 00) , - (0x5f << 16) | (0xff << 8) | (0x97 << 00) , - (0x60 << 16) | (0xff << 8) | (0x97 << 00) , - (0x60 << 16) | (0xff << 8) | (0x97 << 00) , - (0x60 << 16) | (0xff << 8) | (0x96 << 00) , - (0x63 << 16) | (0xff << 8) | (0x94 << 00) , - (0x63 << 16) | (0xff << 8) | (0x94 << 00) , - (0x63 << 16) | (0xff << 8) | (0x94 << 00) , - (0x63 << 16) | (0xff << 8) | (0x94 << 00) , - (0x65 << 16) | (0xff << 8) | (0x90 << 00) , - (0x66 << 16) | (0xff << 8) | (0x90 << 00) , - (0x66 << 16) | (0xff << 8) | (0x90 << 00) , - (0x66 << 16) | (0xff << 8) | (0x8f << 00) , - (0x6a << 16) | (0xff << 8) | (0x8d << 00) , - (0x6a << 16) | (0xff << 8) | (0x8d << 00) , - (0x6a << 16) | (0xff << 8) | (0x8d << 00) , - (0x6c << 16) | (0xff << 8) | (0x8a << 00) , - (0x6d << 16) | (0xff << 8) | (0x8a << 00) , - (0x6d << 16) | (0xff << 8) | (0x8a << 00) , - (0x6d << 16) | (0xff << 8) | (0x8a << 00) , - (0x70 << 16) | (0xff << 8) | (0x87 << 00) , - (0x70 << 16) | (0xff << 8) | (0x87 << 00) , - (0x70 << 16) | (0xff << 8) | (0x87 << 00) , - (0x70 << 16) | (0xff << 8) | (0x87 << 00) , - (0x72 << 16) | (0xff << 8) | (0x83 << 00) , - (0x73 << 16) | (0xff << 8) | (0x83 << 00) , - (0x73 << 16) | (0xff << 8) | (0x83 << 00) , - (0x73 << 16) | (0xff << 8) | (0x83 << 00) , - (0x77 << 16) | (0xff << 8) | (0x80 << 00) , - (0x77 << 16) | (0xff << 8) | (0x80 << 00) , - (0x77 << 16) | (0xff << 8) | (0x80 << 00) , - (0x77 << 16) | (0xff << 8) | (0x80 << 00) , - (0x79 << 16) | (0xff << 8) | (0x7d << 00) , - (0x7a << 16) | (0xff << 8) | (0x7d << 00) , - (0x7a << 16) | (0xff << 8) | (0x7d << 00) , - (0x7a << 16) | (0xff << 8) | (0x7c << 00) , - (0x7d << 16) | (0xff << 8) | (0x7a << 00) , - (0x7d << 16) | (0xff << 8) | (0x7a << 00) , - (0x7d << 16) | (0xff << 8) | (0x7a << 00) , - (0x7d << 16) | (0xff << 8) | (0x7a << 00) , - (0x80 << 16) | (0xff << 8) | (0x77 << 00) , - (0x80 << 16) | (0xff << 8) | (0x77 << 00) , - (0x80 << 16) | (0xff << 8) | (0x77 << 00) , - (0x80 << 16) | (0xff << 8) | (0x77 << 00) , - (0x81 << 16) | (0xff << 8) | (0x74 << 00) , - (0x83 << 16) | (0xff << 8) | (0x73 << 00) , - (0x83 << 16) | (0xff << 8) | (0x73 << 00) , - (0x83 << 16) | (0xff << 8) | (0x73 << 00) , - (0x87 << 16) | (0xff << 8) | (0x70 << 00) , - (0x87 << 16) | (0xff << 8) | (0x70 << 00) , - (0x87 << 16) | (0xff << 8) | (0x70 << 00) , - (0x87 << 16) | (0xff << 8) | (0x70 << 00) , - (0x89 << 16) | (0xff << 8) | (0x6d << 00) , - (0x8a << 16) | (0xff << 8) | (0x6d << 00) , - (0x8a << 16) | (0xff << 8) | (0x6d << 00) , - (0x8a << 16) | (0xff << 8) | (0x6d << 00) , - (0x8d << 16) | (0xff << 8) | (0x6a << 00) , - (0x8d << 16) | (0xff << 8) | (0x6a << 00) , - (0x8d << 16) | (0xff << 8) | (0x69 << 00) , - (0x90 << 16) | (0xff << 8) | (0x66 << 00) , - (0x90 << 16) | (0xff << 8) | (0x66 << 00) , - (0x90 << 16) | (0xff << 8) | (0x66 << 00) , - (0x90 << 16) | (0xff << 8) | (0x66 << 00) , - (0x94 << 16) | (0xff << 8) | (0x63 << 00) , - (0x94 << 16) | (0xff << 8) | (0x63 << 00) , - (0x94 << 16) | (0xff << 8) | (0x63 << 00) , - (0x94 << 16) | (0xff << 8) | (0x63 << 00) , - (0x95 << 16) | (0xff << 8) | (0x61 << 00) , - (0x97 << 16) | (0xff << 8) | (0x60 << 00) , - (0x97 << 16) | (0xff << 8) | (0x60 << 00) , - (0x97 << 16) | (0xff << 8) | (0x60 << 00) , - (0x9a << 16) | (0xff << 8) | (0x5d << 00) , - (0x9a << 16) | (0xff << 8) | (0x5d << 00) , - (0x9a << 16) | (0xff << 8) | (0x5d << 00) , - (0x9a << 16) | (0xff << 8) | (0x5d << 00) , - (0x9c << 16) | (0xff << 8) | (0x5a << 00) , - (0x9d << 16) | (0xff << 8) | (0x5a << 00) , - (0x9d << 16) | (0xff << 8) | (0x5a << 00) , - (0x9d << 16) | (0xff << 8) | (0x5a << 00) , - (0xa0 << 16) | (0xff << 8) | (0x56 << 00) , - (0xa0 << 16) | (0xff << 8) | (0x56 << 00) , - (0xa0 << 16) | (0xff << 8) | (0x56 << 00) , - (0xa0 << 16) | (0xff << 8) | (0x56 << 00) , - (0xa3 << 16) | (0xff << 8) | (0x53 << 00) , - (0xa4 << 16) | (0xff << 8) | (0x53 << 00) , - (0xa4 << 16) | (0xff << 8) | (0x53 << 00) , - (0xa4 << 16) | (0xff << 8) | (0x53 << 00) , - (0xa7 << 16) | (0xff << 8) | (0x50 << 00) , - (0xa7 << 16) | (0xff << 8) | (0x50 << 00) , - (0xa7 << 16) | (0xff << 8) | (0x50 << 00) , - (0xa7 << 16) | (0xff << 8) | (0x50 << 00) , - (0xa9 << 16) | (0xff << 8) | (0x4d << 00) , - (0xaa << 16) | (0xff << 8) | (0x4d << 00) , - (0xaa << 16) | (0xff << 8) | (0x4d << 00) , - (0xaa << 16) | (0xff << 8) | (0x4d << 00) , - (0xab << 16) | (0xff << 8) | (0x4a << 00) , - (0xad << 16) | (0xff << 8) | (0x49 << 00) , - (0xad << 16) | (0xff << 8) | (0x49 << 00) , - (0xaf << 16) | (0xff << 8) | (0x46 << 00) , - (0xb1 << 16) | (0xff << 8) | (0x46 << 00) , - (0xb1 << 16) | (0xff << 8) | (0x46 << 00) , - (0xb1 << 16) | (0xff << 8) | (0x46 << 00) , - (0xb4 << 16) | (0xff << 8) | (0x43 << 00) , - (0xb4 << 16) | (0xff << 8) | (0x43 << 00) , - (0xb4 << 16) | (0xff << 8) | (0x43 << 00) , - (0xb4 << 16) | (0xff << 8) | (0x43 << 00) , - (0xb6 << 16) | (0xff << 8) | (0x40 << 00) , - (0xb7 << 16) | (0xff << 8) | (0x40 << 00) , - (0xb7 << 16) | (0xff << 8) | (0x40 << 00) , - (0xb7 << 16) | (0xff << 8) | (0x40 << 00) , - (0xba << 16) | (0xff << 8) | (0x3c << 00) , - (0xba << 16) | (0xff << 8) | (0x3c << 00) , - (0xba << 16) | (0xff << 8) | (0x3c << 00) , - (0xba << 16) | (0xff << 8) | (0x3c << 00) , - (0xbd << 16) | (0xff << 8) | (0x39 << 00) , - (0xbe << 16) | (0xff << 8) | (0x39 << 00) , - (0xbe << 16) | (0xff << 8) | (0x39 << 00) , - (0xbe << 16) | (0xff << 8) | (0x39 << 00) , - (0xbf << 16) | (0xff << 8) | (0x37 << 00) , - (0xc1 << 16) | (0xff << 8) | (0x36 << 00) , - (0xc1 << 16) | (0xff << 8) | (0x36 << 00) , - (0xc1 << 16) | (0xff << 8) | (0x36 << 00) , - (0xc4 << 16) | (0xff << 8) | (0x33 << 00) , - (0xc4 << 16) | (0xff << 8) | (0x33 << 00) , - (0xc4 << 16) | (0xff << 8) | (0x33 << 00) , - (0xc4 << 16) | (0xff << 8) | (0x33 << 00) , - (0xc5 << 16) | (0xff << 8) | (0x31 << 00) , - (0xc7 << 16) | (0xff << 8) | (0x30 << 00) , - (0xc7 << 16) | (0xff << 8) | (0x30 << 00) , - (0xc7 << 16) | (0xff << 8) | (0x30 << 00) , - (0xca << 16) | (0xff << 8) | (0x2c << 00) , - (0xca << 16) | (0xff << 8) | (0x2c << 00) , - (0xca << 16) | (0xff << 8) | (0x2c << 00) , - (0xca << 16) | (0xff << 8) | (0x2c << 00) , - (0xcc << 16) | (0xff << 8) | (0x29 << 00) , - (0xce << 16) | (0xff << 8) | (0x29 << 00) , - (0xce << 16) | (0xff << 8) | (0x29 << 00) , - (0xd0 << 16) | (0xff << 8) | (0x26 << 00) , - (0xd1 << 16) | (0xff << 8) | (0x26 << 00) , - (0xd1 << 16) | (0xff << 8) | (0x26 << 00) , - (0xd1 << 16) | (0xff << 8) | (0x26 << 00) , - (0xd2 << 16) | (0xff << 8) | (0x24 << 00) , - (0xd4 << 16) | (0xff << 8) | (0x23 << 00) , - (0xd4 << 16) | (0xff << 8) | (0x23 << 00) , - (0xd4 << 16) | (0xff << 8) | (0x23 << 00) , - (0xd7 << 16) | (0xff << 8) | (0x1f << 00) , - (0xd7 << 16) | (0xff << 8) | (0x1f << 00) , - (0xd7 << 16) | (0xff << 8) | (0x1f << 00) , - (0xd7 << 16) | (0xff << 8) | (0x1f << 00) , - (0xd9 << 16) | (0xff << 8) | (0x1d << 00) , - (0xdb << 16) | (0xff << 8) | (0x1c << 00) , - (0xdb << 16) | (0xff << 8) | (0x1c << 00) , - (0xdb << 16) | (0xff << 8) | (0x1c << 00) , - (0xde << 16) | (0xff << 8) | (0x19 << 00) , - (0xde << 16) | (0xff << 8) | (0x19 << 00) , - (0xde << 16) | (0xff << 8) | (0x19 << 00) , - (0xde << 16) | (0xff << 8) | (0x19 << 00) , - (0xe0 << 16) | (0xff << 8) | (0x00 << 00) , - (0xe1 << 16) | (0xff << 8) | (0x00 << 00) , - (0xe1 << 16) | (0xff << 8) | (0x00 << 00) , - (0xe1 << 16) | (0xff << 8) | (0x00 << 00) , - (0xe4 << 16) | (0xff << 8) | (0x13 << 00) , - (0xe4 << 16) | (0xff << 8) | (0x13 << 00) , - (0xe4 << 16) | (0xff << 8) | (0x13 << 00) , - (0xe4 << 16) | (0xff << 8) | (0x13 << 00) , - (0xe6 << 16) | (0xff << 8) | (0x0f << 00) , - (0xe7 << 16) | (0xff << 8) | (0x0f << 00) , - (0xe7 << 16) | (0xff << 8) | (0x0f << 00) , - (0xe7 << 16) | (0xff << 8) | (0x0f << 00) , - (0xeb << 16) | (0xff << 8) | (0x0c << 00) , - (0xeb << 16) | (0xff << 8) | (0x0c << 00) , - (0xeb << 16) | (0xff << 8) | (0x0c << 00) , - (0xeb << 16) | (0xff << 8) | (0x0c << 00) , - (0xed << 16) | (0xff << 8) | (0x09 << 00) , - (0xee << 16) | (0xff << 8) | (0x09 << 00) , - (0xee << 16) | (0xff << 8) | (0x09 << 00) , - (0xf1 << 16) | (0xfc << 8) | (0x06 << 00) , - (0xf1 << 16) | (0xfc << 8) | (0x06 << 00) , - (0xf1 << 16) | (0xfc << 8) | (0x06 << 00) , - (0xf1 << 16) | (0xfc << 8) | (0x06 << 00) , - (0xf2 << 16) | (0xf9 << 8) | (0x03 << 00) , - (0xf4 << 16) | (0xf8 << 8) | (0x02 << 00) , - (0xf4 << 16) | (0xf8 << 8) | (0x02 << 00) , - (0xf4 << 16) | (0xf8 << 8) | (0x02 << 00) , - (0xf8 << 16) | (0xf5 << 8) | (0x00 << 00) , - (0xf8 << 16) | (0xf5 << 8) | (0x00 << 00) , - (0xf8 << 16) | (0xf5 << 8) | (0x00 << 00) , - (0xf8 << 16) | (0xf5 << 8) | (0x00 << 00) , - (0xfa << 16) | (0xf1 << 8) | (0x00 << 00) , - (0xfb << 16) | (0xf1 << 8) | (0x00 << 00) , - (0xfb << 16) | (0xf1 << 8) | (0x00 << 00) , - (0xfb << 16) | (0xf1 << 8) | (0x00 << 00) , - (0xfe << 16) | (0xed << 8) | (0x00 << 00) , - (0xfe << 16) | (0xed << 8) | (0x00 << 00) , - (0xfe << 16) | (0xed << 8) | (0x00 << 00) , - (0xfe << 16) | (0xed << 8) | (0x00 << 00) , - (0xfe << 16) | (0xea << 8) | (0x00 << 00) , - (0xff << 16) | (0xea << 8) | (0x00 << 00) , - (0xff << 16) | (0xea << 8) | (0x00 << 00) , - (0xff << 16) | (0xea << 8) | (0x00 << 00) , - (0xff << 16) | (0xe8 << 8) | (0x00 << 00) , - (0xff << 16) | (0xe6 << 8) | (0x00 << 00) , - (0xff << 16) | (0xe6 << 8) | (0x00 << 00) , - (0xff << 16) | (0xe6 << 8) | (0x00 << 00) , - (0xff << 16) | (0xe2 << 8) | (0x00 << 00) , - (0xff << 16) | (0xe2 << 8) | (0x00 << 00) , - (0xff << 16) | (0xe2 << 8) | (0x00 << 00) , - (0xff << 16) | (0xe2 << 8) | (0x00 << 00) , - (0xff << 16) | (0xdf << 8) | (0x00 << 00) , - (0xff << 16) | (0xde << 8) | (0x00 << 00) , - (0xff << 16) | (0xde << 8) | (0x00 << 00) , - (0xff << 16) | (0xde << 8) | (0x00 << 00) , - (0xff << 16) | (0xdb << 8) | (0x00 << 00) , - (0xff << 16) | (0xdb << 8) | (0x00 << 00) , - (0xff << 16) | (0xdb << 8) | (0x00 << 00) , - (0xff << 16) | (0xdb << 8) | (0x00 << 00) , - (0xff << 16) | (0xd8 << 8) | (0x00 << 00) , - (0xff << 16) | (0xd7 << 8) | (0x00 << 00) , - (0xff << 16) | (0xd7 << 8) | (0x00 << 00) , - (0xff << 16) | (0xd3 << 8) | (0x00 << 00) , - (0xff << 16) | (0xd3 << 8) | (0x00 << 00) , - (0xff << 16) | (0xd3 << 8) | (0x00 << 00) , - (0xff << 16) | (0xd3 << 8) | (0x00 << 00) , - (0xff << 16) | (0xd1 << 8) | (0x00 << 00) , - (0xff << 16) | (0xd0 << 8) | (0x00 << 00) , - (0xff << 16) | (0xd0 << 8) | (0x00 << 00) , - (0xff << 16) | (0xd0 << 8) | (0x00 << 00) , - (0xff << 16) | (0xcc << 8) | (0x00 << 00) , - (0xff << 16) | (0xcc << 8) | (0x00 << 00) , - (0xff << 16) | (0xcc << 8) | (0x00 << 00) , - (0xff << 16) | (0xcc << 8) | (0x00 << 00) , - (0xff << 16) | (0xc9 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc8 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc8 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc8 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc4 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc4 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc4 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc4 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc1 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc1 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc1 << 8) | (0x00 << 00) , - (0xff << 16) | (0xc1 << 8) | (0x00 << 00) , - (0xff << 16) | (0xbd << 8) | (0x00 << 00) , - (0xff << 16) | (0xbd << 8) | (0x00 << 00) , - (0xff << 16) | (0xbd << 8) | (0x00 << 00) , - (0xff << 16) | (0xbd << 8) | (0x00 << 00) , - (0xff << 16) | (0xb9 << 8) | (0x00 << 00) , - (0xff << 16) | (0xb9 << 8) | (0x00 << 00) , - (0xff << 16) | (0xb9 << 8) | (0x00 << 00) , - (0xff << 16) | (0xb9 << 8) | (0x00 << 00) , - (0xff << 16) | (0xb7 << 8) | (0x00 << 00) , - (0xff << 16) | (0xb6 << 8) | (0x00 << 00) , - (0xff << 16) | (0xb6 << 8) | (0x00 << 00) , - (0xff << 16) | (0xb6 << 8) | (0x00 << 00) , - (0xff << 16) | (0xb2 << 8) | (0x00 << 00) , - (0xff << 16) | (0xb2 << 8) | (0x00 << 00) , - (0xff << 16) | (0xb2 << 8) | (0x00 << 00) , - (0xff << 16) | (0xae << 8) | (0x00 << 00) , - (0xff << 16) | (0xae << 8) | (0x00 << 00) , - (0xff << 16) | (0xae << 8) | (0x00 << 00) , - (0xff << 16) | (0xae << 8) | (0x00 << 00) , - (0xff << 16) | (0xac << 8) | (0x00 << 00) , - (0xff << 16) | (0xab << 8) | (0x00 << 00) , - (0xff << 16) | (0xab << 8) | (0x00 << 00) , - (0xff << 16) | (0xab << 8) | (0x00 << 00) , - (0xff << 16) | (0xa7 << 8) | (0x00 << 00) , - (0xff << 16) | (0xa7 << 8) | (0x00 << 00) , - (0xff << 16) | (0xa7 << 8) | (0x00 << 00) , - (0xff << 16) | (0xa7 << 8) | (0x00 << 00) , - (0xff << 16) | (0xa3 << 8) | (0x00 << 00) , - (0xff << 16) | (0xa3 << 8) | (0x00 << 00) , - (0xff << 16) | (0xa3 << 8) | (0x00 << 00) , - (0xff << 16) | (0xa3 << 8) | (0x00 << 00) , - (0xff << 16) | (0xa1 << 8) | (0x00 << 00) , - (0xff << 16) | (0x9f << 8) | (0x00 << 00) , - (0xff << 16) | (0x9f << 8) | (0x00 << 00) , - (0xff << 16) | (0x9f << 8) | (0x00 << 00) , - (0xff << 16) | (0x9c << 8) | (0x00 << 00) , - (0xff << 16) | (0x9c << 8) | (0x00 << 00) , - (0xff << 16) | (0x9c << 8) | (0x00 << 00) , - (0xff << 16) | (0x9c << 8) | (0x00 << 00) , - (0xff << 16) | (0x9a << 8) | (0x00 << 00) , - (0xff << 16) | (0x98 << 8) | (0x00 << 00) , - (0xff << 16) | (0x98 << 8) | (0x00 << 00) , - (0xff << 16) | (0x98 << 8) | (0x00 << 00) , - (0xff << 16) | (0x94 << 8) | (0x00 << 00) , - (0xff << 16) | (0x94 << 8) | (0x00 << 00) , - (0xff << 16) | (0x94 << 8) | (0x00 << 00) , - (0xff << 16) | (0x94 << 8) | (0x00 << 00) , - (0xff << 16) | (0x92 << 8) | (0x00 << 00) , - (0xff << 16) | (0x91 << 8) | (0x00 << 00) , - (0xff << 16) | (0x91 << 8) | (0x00 << 00) , - (0xff << 16) | (0x91 << 8) | (0x00 << 00) , - (0xff << 16) | (0x8d << 8) | (0x00 << 00) , - (0xff << 16) | (0x8d << 8) | (0x00 << 00) , - (0xff << 16) | (0x8d << 8) | (0x00 << 00) , - (0xff << 16) | (0x8b << 8) | (0x00 << 00) , - (0xff << 16) | (0x89 << 8) | (0x00 << 00) , - (0xff << 16) | (0x89 << 8) | (0x00 << 00) , - (0xff << 16) | (0x89 << 8) | (0x00 << 00) , - (0xff << 16) | (0x86 << 8) | (0x00 << 00) , - (0xff << 16) | (0x86 << 8) | (0x00 << 00) , - (0xff << 16) | (0x86 << 8) | (0x00 << 00) , - (0xff << 16) | (0x86 << 8) | (0x00 << 00) , - (0xff << 16) | (0x84 << 8) | (0x00 << 00) , - (0xff << 16) | (0x82 << 8) | (0x00 << 00) , - (0xff << 16) | (0x82 << 8) | (0x00 << 00) , - (0xff << 16) | (0x82 << 8) | (0x00 << 00) , - (0xff << 16) | (0x7e << 8) | (0x00 << 00) , - (0xff << 16) | (0x7e << 8) | (0x00 << 00) , - (0xff << 16) | (0x7e << 8) | (0x00 << 00) , - (0xff << 16) | (0x7e << 8) | (0x00 << 00) , - (0xff << 16) | (0x7b << 8) | (0x00 << 00) , - (0xff << 16) | (0x7a << 8) | (0x00 << 00) , - (0xff << 16) | (0x7a << 8) | (0x00 << 00) , - (0xff << 16) | (0x7a << 8) | (0x00 << 00) , - (0xff << 16) | (0x77 << 8) | (0x00 << 00) , - (0xff << 16) | (0x77 << 8) | (0x00 << 00) , - (0xff << 16) | (0x77 << 8) | (0x00 << 00) , - (0xff << 16) | (0x77 << 8) | (0x00 << 00) , - (0xff << 16) | (0x74 << 8) | (0x00 << 00) , - (0xff << 16) | (0x73 << 8) | (0x00 << 00) , - (0xff << 16) | (0x73 << 8) | (0x00 << 00) , - (0xff << 16) | (0x73 << 8) | (0x00 << 00) , - (0xff << 16) | (0x6f << 8) | (0x00 << 00) , - (0xff << 16) | (0x6f << 8) | (0x00 << 00) , - (0xff << 16) | (0x6f << 8) | (0x00 << 00) , - (0xff << 16) | (0x6f << 8) | (0x00 << 00) , - (0xff << 16) | (0x6c << 8) | (0x00 << 00) , - (0xff << 16) | (0x6c << 8) | (0x00 << 00) , - (0xff << 16) | (0x6c << 8) | (0x00 << 00) , - (0xff << 16) | (0x6c << 8) | (0x00 << 00) , - (0xff << 16) | (0x6a << 8) | (0x00 << 00) , - (0xff << 16) | (0x68 << 8) | (0x00 << 00) , - (0xff << 16) | (0x68 << 8) | (0x00 << 00) , - (0xff << 16) | (0x66 << 8) | (0x00 << 00) , - (0xff << 16) | (0x64 << 8) | (0x00 << 00) , - (0xff << 16) | (0x64 << 8) | (0x00 << 00) , - (0xff << 16) | (0x64 << 8) | (0x00 << 00) , - (0xff << 16) | (0x60 << 8) | (0x00 << 00) , - (0xff << 16) | (0x60 << 8) | (0x00 << 00) , - (0xff << 16) | (0x60 << 8) | (0x00 << 00) , - (0xff << 16) | (0x60 << 8) | (0x00 << 00) , - (0xff << 16) | (0x5e << 8) | (0x00 << 00) , - (0xff << 16) | (0x5d << 8) | (0x00 << 00) , - (0xff << 16) | (0x5d << 8) | (0x00 << 00) , - (0xff << 16) | (0x5d << 8) | (0x00 << 00) , - (0xff << 16) | (0x59 << 8) | (0x00 << 00) , - (0xff << 16) | (0x59 << 8) | (0x00 << 00) , - (0xff << 16) | (0x59 << 8) | (0x00 << 00) , - (0xff << 16) | (0x59 << 8) | (0x00 << 00) , - (0xff << 16) | (0x56 << 8) | (0x00 << 00) , - (0xff << 16) | (0x55 << 8) | (0x00 << 00) , - (0xff << 16) | (0x55 << 8) | (0x00 << 00) , - (0xff << 16) | (0x55 << 8) | (0x00 << 00) , - (0xff << 16) | (0x54 << 8) | (0x00 << 00) , - (0xff << 16) | (0x52 << 8) | (0x00 << 00) , - (0xff << 16) | (0x52 << 8) | (0x00 << 00) , - (0xff << 16) | (0x52 << 8) | (0x00 << 00) , - (0xff << 16) | (0x4e << 8) | (0x00 << 00) , - (0xff << 16) | (0x4e << 8) | (0x00 << 00) , - (0xff << 16) | (0x4e << 8) | (0x00 << 00) , - (0xff << 16) | (0x4e << 8) | (0x00 << 00) , - (0xff << 16) | (0x4c << 8) | (0x00 << 00) , - (0xff << 16) | (0x4a << 8) | (0x00 << 00) , - (0xff << 16) | (0x4a << 8) | (0x00 << 00) , - (0xff << 16) | (0x4a << 8) | (0x00 << 00) , - (0xff << 16) | (0x47 << 8) | (0x00 << 00) , - (0xff << 16) | (0x47 << 8) | (0x00 << 00) , - (0xff << 16) | (0x47 << 8) | (0x00 << 00) , - (0xff << 16) | (0x47 << 8) | (0x00 << 00) , - (0xff << 16) | (0x44 << 8) | (0x00 << 00) , - (0xff << 16) | (0x43 << 8) | (0x00 << 00) , - (0xff << 16) | (0x43 << 8) | (0x00 << 00) , - (0xff << 16) | (0x43 << 8) | (0x00 << 00) , - (0xff << 16) | (0x3f << 8) | (0x00 << 00) , - (0xff << 16) | (0x3f << 8) | (0x00 << 00) , - (0xff << 16) | (0x3f << 8) | (0x00 << 00) , - (0xff << 16) | (0x3d << 8) | (0x00 << 00) , - (0xff << 16) | (0x3b << 8) | (0x00 << 00) , - (0xff << 16) | (0x3b << 8) | (0x00 << 00) , - (0xff << 16) | (0x3b << 8) | (0x00 << 00) , - (0xff << 16) | (0x38 << 8) | (0x00 << 00) , - (0xff << 16) | (0x38 << 8) | (0x00 << 00) , - (0xff << 16) | (0x38 << 8) | (0x00 << 00) , - (0xff << 16) | (0x38 << 8) | (0x00 << 00) , - (0xff << 16) | (0x36 << 8) | (0x00 << 00) , - (0xff << 16) | (0x34 << 8) | (0x00 << 00) , - (0xff << 16) | (0x34 << 8) | (0x00 << 00) , - (0xff << 16) | (0x34 << 8) | (0x00 << 00) , - (0xff << 16) | (0x30 << 8) | (0x00 << 00) , - (0xff << 16) | (0x30 << 8) | (0x00 << 00) , - (0xff << 16) | (0x30 << 8) | (0x00 << 00) , - (0xff << 16) | (0x30 << 8) | (0x00 << 00) , - (0xff << 16) | (0x2e << 8) | (0x00 << 00) , - (0xff << 16) | (0x2d << 8) | (0x00 << 00) , - (0xff << 16) | (0x2d << 8) | (0x00 << 00) , - (0xff << 16) | (0x2d << 8) | (0x00 << 00) , - (0xff << 16) | (0x29 << 8) | (0x00 << 00) , - (0xff << 16) | (0x29 << 8) | (0x00 << 00) , - (0xff << 16) | (0x29 << 8) | (0x00 << 00) , - (0xff << 16) | (0x29 << 8) | (0x00 << 00) , - (0xff << 16) | (0x26 << 8) | (0x00 << 00) , - (0xff << 16) | (0x25 << 8) | (0x00 << 00) , - (0xff << 16) | (0x25 << 8) | (0x00 << 00) , - (0xff << 16) | (0x25 << 8) | (0x00 << 00) , - (0xff << 16) | (0x24 << 8) | (0x00 << 00) , - (0xff << 16) | (0x22 << 8) | (0x00 << 00) , - (0xff << 16) | (0x22 << 8) | (0x00 << 00) , - (0xff << 16) | (0x22 << 8) | (0x00 << 00) , - (0xff << 16) | (0x1f << 8) | (0x00 << 00) , - (0xff << 16) | (0x1e << 8) | (0x00 << 00) , - (0xff << 16) | (0x1e << 8) | (0x00 << 00) , - (0xff << 16) | (0x1e << 8) | (0x00 << 00) , - (0xff << 16) | (0x1c << 8) | (0x00 << 00) , - (0xff << 16) | (0x1a << 8) | (0x00 << 00) , - (0xff << 16) | (0x1a << 8) | (0x00 << 00) , - (0xff << 16) | (0x18 << 8) | (0x00 << 00) , - (0xff << 16) | (0x00 << 8) | (0x00 << 00) , - (0xff << 16) | (0x00 << 8) | (0x00 << 00) , - (0xff << 16) | (0x00 << 8) | (0x00 << 00) , - (0xff << 16) | (0x13 << 8) | (0x00 << 00) , - (0xff << 16) | (0x13 << 8) | (0x00 << 00) , - (0xff << 16) | (0x13 << 8) | (0x00 << 00) , - (0xff << 16) | (0x13 << 8) | (0x00 << 00) , - (0xfc << 16) | (0x10 << 8) | (0x00 << 00) , - (0xfa << 16) | (0x0f << 8) | (0x00 << 00) , - (0xfa << 16) | (0x0f << 8) | (0x00 << 00) , - (0xfa << 16) | (0x0f << 8) | (0x00 << 00) , - (0xf9 << 16) | (0x0e << 8) | (0x00 << 00) , - (0xf6 << 16) | (0x0b << 8) | (0x00 << 00) , - (0xf6 << 16) | (0x0b << 8) | (0x00 << 00) , - (0xf6 << 16) | (0x0b << 8) | (0x00 << 00) , - (0xf2 << 16) | (0x08 << 8) | (0x00 << 00) , - (0xf1 << 16) | (0x08 << 8) | (0x00 << 00) , - (0xf1 << 16) | (0x08 << 8) | (0x00 << 00) , - (0xf1 << 16) | (0x08 << 8) | (0x00 << 00) , - (0xef << 16) | (0x06 << 8) | (0x00 << 00) , - (0xed << 16) | (0x04 << 8) | (0x00 << 00) , - (0xed << 16) | (0x04 << 8) | (0x00 << 00) , - (0xed << 16) | (0x04 << 8) | (0x00 << 00) , - (0xe8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xe8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xe8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xe8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xe6 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xe4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xe4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xe4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xdf << 16) | (0x00 << 8) | (0x00 << 00) , - (0xdf << 16) | (0x00 << 8) | (0x00 << 00) , - (0xdf << 16) | (0x00 << 8) | (0x00 << 00) , - (0xdf << 16) | (0x00 << 8) | (0x00 << 00) , - (0xdc << 16) | (0x00 << 8) | (0x00 << 00) , - (0xda << 16) | (0x00 << 8) | (0x00 << 00) , - (0xda << 16) | (0x00 << 8) | (0x00 << 00) , - (0xd7 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xd6 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xd6 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xd6 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xd4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xd1 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xd1 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xd1 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xcd << 16) | (0x00 << 8) | (0x00 << 00) , - (0xcd << 16) | (0x00 << 8) | (0x00 << 00) , - (0xcd << 16) | (0x00 << 8) | (0x00 << 00) , - (0xcd << 16) | (0x00 << 8) | (0x00 << 00) , - (0xcb << 16) | (0x00 << 8) | (0x00 << 00) , - (0xc8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xc8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xc8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xc4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xc4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xc4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xc4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xc1 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xbf << 16) | (0x00 << 8) | (0x00 << 00) , - (0xbf << 16) | (0x00 << 8) | (0x00 << 00) , - (0xbf << 16) | (0x00 << 8) | (0x00 << 00) , - (0xbb << 16) | (0x00 << 8) | (0x00 << 00) , - (0xbb << 16) | (0x00 << 8) | (0x00 << 00) , - (0xbb << 16) | (0x00 << 8) | (0x00 << 00) , - (0xbb << 16) | (0x00 << 8) | (0x00 << 00) , - (0xb7 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xb6 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xb6 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xb6 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xb5 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xb2 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xb2 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xb2 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xae << 16) | (0x00 << 8) | (0x00 << 00) , - (0xad << 16) | (0x00 << 8) | (0x00 << 00) , - (0xad << 16) | (0x00 << 8) | (0x00 << 00) , - (0xa8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xa8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xa8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xa8 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xa6 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xa4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xa4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0xa4 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x9f << 16) | (0x00 << 8) | (0x00 << 00) , - (0x9f << 16) | (0x00 << 8) | (0x00 << 00) , - (0x9f << 16) | (0x00 << 8) | (0x00 << 00) , - (0x9f << 16) | (0x00 << 8) | (0x00 << 00) , - (0x9c << 16) | (0x00 << 8) | (0x00 << 00) , - (0x9b << 16) | (0x00 << 8) | (0x00 << 00) , - (0x9b << 16) | (0x00 << 8) | (0x00 << 00) , - (0x9b << 16) | (0x00 << 8) | (0x00 << 00) , - (0x9a << 16) | (0x00 << 8) | (0x00 << 00) , - (0x96 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x96 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x96 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x93 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x92 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x92 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x92 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x90 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x8d << 16) | (0x00 << 8) | (0x00 << 00) , - (0x8d << 16) | (0x00 << 8) | (0x00 << 00) , - (0x8d << 16) | (0x00 << 8) | (0x00 << 00) , - (0x89 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x89 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x89 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x89 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x87 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x84 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x84 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x84 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x80 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x80 << 16) | (0x00 << 8) | (0x00 << 00) , - (0x80 << 16) | (0x00 << 8) | (0x00 << 00) , + (0x00 << 16) | (0x00 << 8) | (0x77 << 00), + (0x00 << 16) | (0x00 << 8) | (0x7f << 00), + (0x00 << 16) | (0x00 << 8) | (0x80 << 00), + (0x00 << 16) | (0x00 << 8) | (0x82 << 00), + (0x00 << 16) | (0x00 << 8) | (0x84 << 00), + (0x00 << 16) | (0x00 << 8) | (0x84 << 00), + (0x00 << 16) | (0x00 << 8) | (0x84 << 00), + (0x00 << 16) | (0x00 << 8) | (0x85 << 00), + (0x00 << 16) | (0x00 << 8) | (0x89 << 00), + (0x00 << 16) | (0x00 << 8) | (0x89 << 00), + (0x00 << 16) | (0x00 << 8) | (0x89 << 00), + (0x00 << 16) | (0x00 << 8) | (0x89 << 00), + (0x00 << 16) | (0x00 << 8) | (0x8d << 00), + (0x00 << 16) | (0x00 << 8) | (0x8d << 00), + (0x00 << 16) | (0x00 << 8) | (0x8d << 00), + (0x00 << 16) | (0x00 << 8) | (0x8e << 00), + (0x00 << 16) | (0x00 << 8) | (0x92 << 00), + (0x00 << 16) | (0x00 << 8) | (0x92 << 00), + (0x00 << 16) | (0x00 << 8) | (0x92 << 00), + (0x00 << 16) | (0x00 << 8) | (0x92 << 00), + (0x00 << 16) | (0x00 << 8) | (0x96 << 00), + (0x00 << 16) | (0x00 << 8) | (0x96 << 00), + (0x00 << 16) | (0x00 << 8) | (0x96 << 00), + (0x00 << 16) | (0x00 << 8) | (0x98 << 00), + (0x00 << 16) | (0x00 << 8) | (0x9b << 00), + (0x00 << 16) | (0x00 << 8) | (0x9b << 00), + (0x00 << 16) | (0x00 << 8) | (0x9b << 00), + (0x00 << 16) | (0x00 << 8) | (0x9b << 00), + (0x00 << 16) | (0x00 << 8) | (0x9f << 00), + (0x00 << 16) | (0x00 << 8) | (0x9f << 00), + (0x00 << 16) | (0x00 << 8) | (0x9f << 00), + (0x00 << 16) | (0x00 << 8) | (0xa1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xa4 << 00), + (0x00 << 16) | (0x00 << 8) | (0xa4 << 00), + (0x00 << 16) | (0x00 << 8) | (0xa4 << 00), + (0x00 << 16) | (0x00 << 8) | (0xa4 << 00), + (0x00 << 16) | (0x00 << 8) | (0xa8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xa8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xa8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xa8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xad << 00), + (0x00 << 16) | (0x00 << 8) | (0xad << 00), + (0x00 << 16) | (0x00 << 8) | (0xad << 00), + (0x00 << 16) | (0x00 << 8) | (0xb1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xb1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xb1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xb3 << 00), + (0x00 << 16) | (0x00 << 8) | (0xb6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xb6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xb6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xb6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xbb << 00), + (0x00 << 16) | (0x00 << 8) | (0xbb << 00), + (0x00 << 16) | (0x00 << 8) | (0xbb << 00), + (0x00 << 16) | (0x00 << 8) | (0xbd << 00), + (0x00 << 16) | (0x00 << 8) | (0xbf << 00), + (0x00 << 16) | (0x00 << 8) | (0xbf << 00), + (0x00 << 16) | (0x00 << 8) | (0xbf << 00), + (0x00 << 16) | (0x00 << 8) | (0xbf << 00), + (0x00 << 16) | (0x00 << 8) | (0xc4 << 00), + (0x00 << 16) | (0x00 << 8) | (0xc4 << 00), + (0x00 << 16) | (0x00 << 8) | (0xc4 << 00), + (0x00 << 16) | (0x00 << 8) | (0xc4 << 00), + (0x00 << 16) | (0x00 << 8) | (0xc8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xc8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xc8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xc9 << 00), + (0x00 << 16) | (0x00 << 8) | (0xcd << 00), + (0x00 << 16) | (0x00 << 8) | (0xcd << 00), + (0x00 << 16) | (0x00 << 8) | (0xcd << 00), + (0x00 << 16) | (0x00 << 8) | (0xcd << 00), + (0x00 << 16) | (0x00 << 8) | (0xd1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xd1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xd1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xd2 << 00), + (0x00 << 16) | (0x00 << 8) | (0xd6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xd6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xd6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xd6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xda << 00), + (0x00 << 16) | (0x00 << 8) | (0xda << 00), + (0x00 << 16) | (0x00 << 8) | (0xda << 00), + (0x00 << 16) | (0x00 << 8) | (0xdf << 00), + (0x00 << 16) | (0x00 << 8) | (0xdf << 00), + (0x00 << 16) | (0x00 << 8) | (0xdf << 00), + (0x00 << 16) | (0x00 << 8) | (0xdf << 00), + (0x00 << 16) | (0x00 << 8) | (0xe3 << 00), + (0x00 << 16) | (0x00 << 8) | (0xe3 << 00), + (0x00 << 16) | (0x00 << 8) | (0xe3 << 00), + (0x00 << 16) | (0x00 << 8) | (0xe4 << 00), + (0x00 << 16) | (0x00 << 8) | (0xe8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xe8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xe8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xe8 << 00), + (0x00 << 16) | (0x00 << 8) | (0xed << 00), + (0x00 << 16) | (0x00 << 8) | (0xed << 00), + (0x00 << 16) | (0x00 << 8) | (0xed << 00), + (0x00 << 16) | (0x00 << 8) | (0xee << 00), + (0x00 << 16) | (0x00 << 8) | (0xf1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xf1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xf1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xf1 << 00), + (0x00 << 16) | (0x00 << 8) | (0xf6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xf6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xf6 << 00), + (0x00 << 16) | (0x00 << 8) | (0xf7 << 00), + (0x00 << 16) | (0x00 << 8) | (0xfa << 00), + (0x00 << 16) | (0x00 << 8) | (0xfa << 00), + (0x00 << 16) | (0x00 << 8) | (0xfa << 00), + (0x00 << 16) | (0x00 << 8) | (0xfa << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x00 << 8) | (0xff << 00), + (0x00 << 16) | (0x01 << 8) | (0xff << 00), + (0x00 << 16) | (0x04 << 8) | (0xff << 00), + (0x00 << 16) | (0x04 << 8) | (0xff << 00), + (0x00 << 16) | (0x04 << 8) | (0xff << 00), + (0x00 << 16) | (0x04 << 8) | (0xff << 00), + (0x00 << 16) | (0x08 << 8) | (0xff << 00), + (0x00 << 16) | (0x08 << 8) | (0xff << 00), + (0x00 << 16) | (0x08 << 8) | (0xff << 00), + (0x00 << 16) | (0x0a << 8) | (0xff << 00), + (0x00 << 16) | (0x0c << 8) | (0xff << 00), + (0x00 << 16) | (0x0c << 8) | (0xff << 00), + (0x00 << 16) | (0x0c << 8) | (0xff << 00), + (0x00 << 16) | (0x0c << 8) | (0xff << 00), + (0x00 << 16) | (0x11 << 8) | (0xff << 00), + (0x00 << 16) | (0x11 << 8) | (0xff << 00), + (0x00 << 16) | (0x11 << 8) | (0xff << 00), + (0x00 << 16) | (0x11 << 8) | (0xff << 00), + (0x00 << 16) | (0x15 << 8) | (0xff << 00), + (0x00 << 16) | (0x15 << 8) | (0xff << 00), + (0x00 << 16) | (0x15 << 8) | (0xff << 00), + (0x00 << 16) | (0x15 << 8) | (0xff << 00), + (0x00 << 16) | (0x19 << 8) | (0xff << 00), + (0x00 << 16) | (0x19 << 8) | (0xff << 00), + (0x00 << 16) | (0x19 << 8) | (0xff << 00), + (0x00 << 16) | (0x19 << 8) | (0xff << 00), + (0x00 << 16) | (0x1d << 8) | (0xff << 00), + (0x00 << 16) | (0x1d << 8) | (0xff << 00), + (0x00 << 16) | (0x1d << 8) | (0xff << 00), + (0x00 << 16) | (0x1e << 8) | (0xff << 00), + (0x00 << 16) | (0x20 << 8) | (0xff << 00), + (0x00 << 16) | (0x20 << 8) | (0xff << 00), + (0x00 << 16) | (0x22 << 8) | (0xff << 00), + (0x00 << 16) | (0x24 << 8) | (0xff << 00), + (0x00 << 16) | (0x24 << 8) | (0xff << 00), + (0x00 << 16) | (0x24 << 8) | (0xff << 00), + (0x00 << 16) | (0x24 << 8) | (0xff << 00), + (0x00 << 16) | (0x28 << 8) | (0xff << 00), + (0x00 << 16) | (0x28 << 8) | (0xff << 00), + (0x00 << 16) | (0x28 << 8) | (0xff << 00), + (0x00 << 16) | (0x28 << 8) | (0xff << 00), + (0x00 << 16) | (0x2c << 8) | (0xff << 00), + (0x00 << 16) | (0x2c << 8) | (0xff << 00), + (0x00 << 16) | (0x2c << 8) | (0xff << 00), + (0x00 << 16) | (0x2c << 8) | (0xff << 00), + (0x00 << 16) | (0x30 << 8) | (0xff << 00), + (0x00 << 16) | (0x30 << 8) | (0xff << 00), + (0x00 << 16) | (0x30 << 8) | (0xff << 00), + (0x00 << 16) | (0x30 << 8) | (0xff << 00), + (0x00 << 16) | (0x34 << 8) | (0xff << 00), + (0x00 << 16) | (0x34 << 8) | (0xff << 00), + (0x00 << 16) | (0x34 << 8) | (0xff << 00), + (0x00 << 16) | (0x35 << 8) | (0xff << 00), + (0x00 << 16) | (0x38 << 8) | (0xff << 00), + (0x00 << 16) | (0x38 << 8) | (0xff << 00), + (0x00 << 16) | (0x38 << 8) | (0xff << 00), + (0x00 << 16) | (0x38 << 8) | (0xff << 00), + (0x00 << 16) | (0x3c << 8) | (0xff << 00), + (0x00 << 16) | (0x3c << 8) | (0xff << 00), + (0x00 << 16) | (0x3c << 8) | (0xff << 00), + (0x00 << 16) | (0x3d << 8) | (0xff << 00), + (0x00 << 16) | (0x40 << 8) | (0xff << 00), + (0x00 << 16) | (0x40 << 8) | (0xff << 00), + (0x00 << 16) | (0x40 << 8) | (0xff << 00), + (0x00 << 16) | (0x40 << 8) | (0xff << 00), + (0x00 << 16) | (0x44 << 8) | (0xff << 00), + (0x00 << 16) | (0x44 << 8) | (0xff << 00), + (0x00 << 16) | (0x44 << 8) | (0xff << 00), + (0x00 << 16) | (0x44 << 8) | (0xff << 00), + (0x00 << 16) | (0x47 << 8) | (0xff << 00), + (0x00 << 16) | (0x48 << 8) | (0xff << 00), + (0x00 << 16) | (0x48 << 8) | (0xff << 00), + (0x00 << 16) | (0x48 << 8) | (0xff << 00), + (0x00 << 16) | (0x4c << 8) | (0xff << 00), + (0x00 << 16) | (0x4c << 8) | (0xff << 00), + (0x00 << 16) | (0x4d << 8) | (0xff << 00), + (0x00 << 16) | (0x50 << 8) | (0xff << 00), + (0x00 << 16) | (0x50 << 8) | (0xff << 00), + (0x00 << 16) | (0x50 << 8) | (0xff << 00), + (0x00 << 16) | (0x50 << 8) | (0xff << 00), + (0x00 << 16) | (0x54 << 8) | (0xff << 00), + (0x00 << 16) | (0x54 << 8) | (0xff << 00), + (0x00 << 16) | (0x54 << 8) | (0xff << 00), + (0x00 << 16) | (0x55 << 8) | (0xff << 00), + (0x00 << 16) | (0x58 << 8) | (0xff << 00), + (0x00 << 16) | (0x58 << 8) | (0xff << 00), + (0x00 << 16) | (0x58 << 8) | (0xff << 00), + (0x00 << 16) | (0x58 << 8) | (0xff << 00), + (0x00 << 16) | (0x5c << 8) | (0xff << 00), + (0x00 << 16) | (0x5c << 8) | (0xff << 00), + (0x00 << 16) | (0x5c << 8) | (0xff << 00), + (0x00 << 16) | (0x5c << 8) | (0xff << 00), + (0x00 << 16) | (0x5f << 8) | (0xff << 00), + (0x00 << 16) | (0x60 << 8) | (0xff << 00), + (0x00 << 16) | (0x60 << 8) | (0xff << 00), + (0x00 << 16) | (0x60 << 8) | (0xff << 00), + (0x00 << 16) | (0x64 << 8) | (0xff << 00), + (0x00 << 16) | (0x64 << 8) | (0xff << 00), + (0x00 << 16) | (0x64 << 8) | (0xff << 00), + (0x00 << 16) | (0x64 << 8) | (0xff << 00), + (0x00 << 16) | (0x68 << 8) | (0xff << 00), + (0x00 << 16) | (0x68 << 8) | (0xff << 00), + (0x00 << 16) | (0x68 << 8) | (0xff << 00), + (0x00 << 16) | (0x68 << 8) | (0xff << 00), + (0x00 << 16) | (0x6c << 8) | (0xff << 00), + (0x00 << 16) | (0x6c << 8) | (0xff << 00), + (0x00 << 16) | (0x6c << 8) | (0xff << 00), + (0x00 << 16) | (0x6c << 8) | (0xff << 00), + (0x00 << 16) | (0x70 << 8) | (0xff << 00), + (0x00 << 16) | (0x70 << 8) | (0xff << 00), + (0x00 << 16) | (0x70 << 8) | (0xff << 00), + (0x00 << 16) | (0x71 << 8) | (0xff << 00), + (0x00 << 16) | (0x74 << 8) | (0xff << 00), + (0x00 << 16) | (0x74 << 8) | (0xff << 00), + (0x00 << 16) | (0x74 << 8) | (0xff << 00), + (0x00 << 16) | (0x77 << 8) | (0xff << 00), + (0x00 << 16) | (0x78 << 8) | (0xff << 00), + (0x00 << 16) | (0x78 << 8) | (0xff << 00), + (0x00 << 16) | (0x78 << 8) | (0xff << 00), + (0x00 << 16) | (0x7c << 8) | (0xff << 00), + (0x00 << 16) | (0x7c << 8) | (0xff << 00), + (0x00 << 16) | (0x7c << 8) | (0xff << 00), + (0x00 << 16) | (0x7c << 8) | (0xff << 00), + (0x00 << 16) | (0x80 << 8) | (0xff << 00), + (0x00 << 16) | (0x80 << 8) | (0xff << 00), + (0x00 << 16) | (0x80 << 8) | (0xff << 00), + (0x00 << 16) | (0x80 << 8) | (0xff << 00), + (0x00 << 16) | (0x85 << 8) | (0xff << 00), + (0x00 << 16) | (0x85 << 8) | (0xff << 00), + (0x00 << 16) | (0x85 << 8) | (0xff << 00), + (0x00 << 16) | (0x85 << 8) | (0xff << 00), + (0x00 << 16) | (0x88 << 8) | (0xff << 00), + (0x00 << 16) | (0x88 << 8) | (0xff << 00), + (0x00 << 16) | (0x88 << 8) | (0xff << 00), + (0x00 << 16) | (0x89 << 8) | (0xff << 00), + (0x00 << 16) | (0x8d << 8) | (0xff << 00), + (0x00 << 16) | (0x8d << 8) | (0xff << 00), + (0x00 << 16) | (0x8d << 8) | (0xff << 00), + (0x00 << 16) | (0x8d << 8) | (0xff << 00), + (0x00 << 16) | (0x90 << 8) | (0xff << 00), + (0x00 << 16) | (0x90 << 8) | (0xff << 00), + (0x00 << 16) | (0x90 << 8) | (0xff << 00), + (0x00 << 16) | (0x92 << 8) | (0xff << 00), + (0x00 << 16) | (0x95 << 8) | (0xff << 00), + (0x00 << 16) | (0x95 << 8) | (0xff << 00), + (0x00 << 16) | (0x95 << 8) | (0xff << 00), + (0x00 << 16) | (0x95 << 8) | (0xff << 00), + (0x00 << 16) | (0x98 << 8) | (0xff << 00), + (0x00 << 16) | (0x98 << 8) | (0xff << 00), + (0x00 << 16) | (0x98 << 8) | (0xff << 00), + (0x00 << 16) | (0x98 << 8) | (0xff << 00), + (0x00 << 16) | (0x9c << 8) | (0xff << 00), + (0x00 << 16) | (0x9d << 8) | (0xff << 00), + (0x00 << 16) | (0x9d << 8) | (0xff << 00), + (0x00 << 16) | (0xa0 << 8) | (0xff << 00), + (0x00 << 16) | (0xa0 << 8) | (0xff << 00), + (0x00 << 16) | (0xa0 << 8) | (0xff << 00), + (0x00 << 16) | (0xa1 << 8) | (0xff << 00), + (0x00 << 16) | (0xa5 << 8) | (0xff << 00), + (0x00 << 16) | (0xa5 << 8) | (0xff << 00), + (0x00 << 16) | (0xa5 << 8) | (0xff << 00), + (0x00 << 16) | (0xa5 << 8) | (0xff << 00), + (0x00 << 16) | (0xa8 << 8) | (0xff << 00), + (0x00 << 16) | (0xa8 << 8) | (0xff << 00), + (0x00 << 16) | (0xa8 << 8) | (0xff << 00), + (0x00 << 16) | (0xa9 << 8) | (0xff << 00), + (0x00 << 16) | (0xad << 8) | (0xff << 00), + (0x00 << 16) | (0xad << 8) | (0xff << 00), + (0x00 << 16) | (0xad << 8) | (0xff << 00), + (0x00 << 16) | (0xad << 8) | (0xff << 00), + (0x00 << 16) | (0xb0 << 8) | (0xff << 00), + (0x00 << 16) | (0xb0 << 8) | (0xff << 00), + (0x00 << 16) | (0xb0 << 8) | (0xff << 00), + (0x00 << 16) | (0xb0 << 8) | (0xff << 00), + (0x00 << 16) | (0xb4 << 8) | (0xff << 00), + (0x00 << 16) | (0xb5 << 8) | (0xff << 00), + (0x00 << 16) | (0xb5 << 8) | (0xff << 00), + (0x00 << 16) | (0xb5 << 8) | (0xff << 00), + (0x00 << 16) | (0xb8 << 8) | (0xff << 00), + (0x00 << 16) | (0xb8 << 8) | (0xff << 00), + (0x00 << 16) | (0xb8 << 8) | (0xff << 00), + (0x00 << 16) | (0xb8 << 8) | (0xff << 00), + (0x00 << 16) | (0xbd << 8) | (0xff << 00), + (0x00 << 16) | (0xbd << 8) | (0xff << 00), + (0x00 << 16) | (0xbd << 8) | (0xff << 00), + (0x00 << 16) | (0xbd << 8) | (0xff << 00), + (0x00 << 16) | (0xc0 << 8) | (0xff << 00), + (0x00 << 16) | (0xc0 << 8) | (0xff << 00), + (0x00 << 16) | (0xc0 << 8) | (0xff << 00), + (0x00 << 16) | (0xc0 << 8) | (0xff << 00), + (0x00 << 16) | (0xc5 << 8) | (0xff << 00), + (0x00 << 16) | (0xc5 << 8) | (0xff << 00), + (0x00 << 16) | (0xc5 << 8) | (0xff << 00), + (0x00 << 16) | (0xc8 << 8) | (0xff << 00), + (0x00 << 16) | (0xc8 << 8) | (0xff << 00), + (0x00 << 16) | (0xc8 << 8) | (0xff << 00), + (0x00 << 16) | (0xc8 << 8) | (0xff << 00), + (0x00 << 16) | (0xcc << 8) | (0xff << 00), + (0x00 << 16) | (0xcd << 8) | (0xff << 00), + (0x00 << 16) | (0xcd << 8) | (0xff << 00), + (0x00 << 16) | (0xcd << 8) | (0xff << 00), + (0x00 << 16) | (0xd0 << 8) | (0xff << 00), + (0x00 << 16) | (0xd0 << 8) | (0xff << 00), + (0x00 << 16) | (0xd0 << 8) | (0xff << 00), + (0x00 << 16) | (0xd0 << 8) | (0xff << 00), + (0x00 << 16) | (0xd4 << 8) | (0xff << 00), + (0x00 << 16) | (0xd5 << 8) | (0xff << 00), + (0x00 << 16) | (0xd5 << 8) | (0xff << 00), + (0x00 << 16) | (0xd5 << 8) | (0xff << 00), + (0x00 << 16) | (0xd8 << 8) | (0xff << 00), + (0x00 << 16) | (0xd8 << 8) | (0xff << 00), + (0x00 << 16) | (0xd8 << 8) | (0xff << 00), + (0x00 << 16) | (0xd8 << 8) | (0xff << 00), + (0x00 << 16) | (0xdd << 8) | (0xfe << 00), + (0x00 << 16) | (0xdd << 8) | (0xfe << 00), + (0x00 << 16) | (0xdd << 8) | (0xfe << 00), + (0x00 << 16) | (0xdd << 8) | (0xfd << 00), + (0x00 << 16) | (0xe0 << 8) | (0xfb << 00), + (0x00 << 16) | (0xe0 << 8) | (0xfb << 00), + (0x00 << 16) | (0xe0 << 8) | (0xfb << 00), + (0x00 << 16) | (0xe0 << 8) | (0xfb << 00), + (0x00 << 16) | (0xe5 << 8) | (0xf8 << 00), + (0x00 << 16) | (0xe5 << 8) | (0xf8 << 00), + (0x00 << 16) | (0xe5 << 8) | (0xf8 << 00), + (0x00 << 16) | (0xe5 << 8) | (0xf8 << 00), + (0x01 << 16) | (0xe7 << 8) | (0xf4 << 00), + (0x02 << 16) | (0xe8 << 8) | (0xf4 << 00), + (0x02 << 16) | (0xe8 << 8) | (0xf4 << 00), + (0x02 << 16) | (0xe8 << 8) | (0xf4 << 00), + (0x06 << 16) | (0xed << 8) | (0xf1 << 00), + (0x06 << 16) | (0xed << 8) | (0xf1 << 00), + (0x06 << 16) | (0xed << 8) | (0xf1 << 00), + (0x06 << 16) | (0xed << 8) | (0xf1 << 00), + (0x08 << 16) | (0xef << 8) | (0xee << 00), + (0x09 << 16) | (0xf0 << 8) | (0xee << 00), + (0x09 << 16) | (0xf0 << 8) | (0xee << 00), + (0x0c << 16) | (0xf5 << 8) | (0xeb << 00), + (0x0c << 16) | (0xf5 << 8) | (0xeb << 00), + (0x0c << 16) | (0xf5 << 8) | (0xeb << 00), + (0x0c << 16) | (0xf5 << 8) | (0xea << 00), + (0x0f << 16) | (0xf8 << 8) | (0xe7 << 00), + (0x0f << 16) | (0xf8 << 8) | (0xe7 << 00), + (0x0f << 16) | (0xf8 << 8) | (0xe7 << 00), + (0x0f << 16) | (0xf8 << 8) | (0xe7 << 00), + (0x13 << 16) | (0xfd << 8) | (0xe4 << 00), + (0x13 << 16) | (0xfd << 8) | (0xe4 << 00), + (0x13 << 16) | (0xfd << 8) | (0xe4 << 00), + (0x13 << 16) | (0xfd << 8) | (0xe4 << 00), + (0x15 << 16) | (0xfe << 8) | (0xe1 << 00), + (0x00 << 16) | (0xff << 8) | (0xe1 << 00), + (0x00 << 16) | (0xff << 8) | (0xe1 << 00), + (0x00 << 16) | (0xff << 8) | (0xe1 << 00), + (0x19 << 16) | (0xff << 8) | (0xde << 00), + (0x19 << 16) | (0xff << 8) | (0xde << 00), + (0x19 << 16) | (0xff << 8) | (0xde << 00), + (0x19 << 16) | (0xff << 8) | (0xde << 00), + (0x1b << 16) | (0xff << 8) | (0xdb << 00), + (0x1c << 16) | (0xff << 8) | (0xdb << 00), + (0x1c << 16) | (0xff << 8) | (0xdb << 00), + (0x1c << 16) | (0xff << 8) | (0xda << 00), + (0x1f << 16) | (0xff << 8) | (0xd7 << 00), + (0x1f << 16) | (0xff << 8) | (0xd7 << 00), + (0x1f << 16) | (0xff << 8) | (0xd7 << 00), + (0x1f << 16) | (0xff << 8) | (0xd7 << 00), + (0x22 << 16) | (0xff << 8) | (0xd4 << 00), + (0x23 << 16) | (0xff << 8) | (0xd4 << 00), + (0x23 << 16) | (0xff << 8) | (0xd4 << 00), + (0x23 << 16) | (0xff << 8) | (0xd3 << 00), + (0x26 << 16) | (0xff << 8) | (0xd1 << 00), + (0x26 << 16) | (0xff << 8) | (0xd1 << 00), + (0x26 << 16) | (0xff << 8) | (0xd1 << 00), + (0x26 << 16) | (0xff << 8) | (0xd1 << 00), + (0x29 << 16) | (0xff << 8) | (0xce << 00), + (0x29 << 16) | (0xff << 8) | (0xce << 00), + (0x29 << 16) | (0xff << 8) | (0xce << 00), + (0x2c << 16) | (0xff << 8) | (0xca << 00), + (0x2c << 16) | (0xff << 8) | (0xca << 00), + (0x2c << 16) | (0xff << 8) | (0xca << 00), + (0x2c << 16) | (0xff << 8) | (0xca << 00), + (0x2f << 16) | (0xff << 8) | (0xc7 << 00), + (0x30 << 16) | (0xff << 8) | (0xc7 << 00), + (0x30 << 16) | (0xff << 8) | (0xc7 << 00), + (0x30 << 16) | (0xff << 8) | (0xc6 << 00), + (0x33 << 16) | (0xff << 8) | (0xc4 << 00), + (0x33 << 16) | (0xff << 8) | (0xc4 << 00), + (0x33 << 16) | (0xff << 8) | (0xc4 << 00), + (0x33 << 16) | (0xff << 8) | (0xc4 << 00), + (0x35 << 16) | (0xff << 8) | (0xc1 << 00), + (0x36 << 16) | (0xff << 8) | (0xc1 << 00), + (0x36 << 16) | (0xff << 8) | (0xc1 << 00), + (0x36 << 16) | (0xff << 8) | (0xc0 << 00), + (0x39 << 16) | (0xff << 8) | (0xbe << 00), + (0x39 << 16) | (0xff << 8) | (0xbe << 00), + (0x39 << 16) | (0xff << 8) | (0xbe << 00), + (0x39 << 16) | (0xff << 8) | (0xbe << 00), + (0x3c << 16) | (0xff << 8) | (0xba << 00), + (0x3c << 16) | (0xff << 8) | (0xba << 00), + (0x3c << 16) | (0xff << 8) | (0xba << 00), + (0x3d << 16) | (0xff << 8) | (0xb9 << 00), + (0x40 << 16) | (0xff << 8) | (0xb7 << 00), + (0x40 << 16) | (0xff << 8) | (0xb7 << 00), + (0x40 << 16) | (0xff << 8) | (0xb7 << 00), + (0x40 << 16) | (0xff << 8) | (0xb7 << 00), + (0x43 << 16) | (0xff << 8) | (0xb4 << 00), + (0x43 << 16) | (0xff << 8) | (0xb4 << 00), + (0x43 << 16) | (0xff << 8) | (0xb4 << 00), + (0x43 << 16) | (0xff << 8) | (0xb4 << 00), + (0x45 << 16) | (0xff << 8) | (0xb1 << 00), + (0x46 << 16) | (0xff << 8) | (0xb1 << 00), + (0x46 << 16) | (0xff << 8) | (0xb1 << 00), + (0x46 << 16) | (0xff << 8) | (0xb1 << 00), + (0x49 << 16) | (0xff << 8) | (0xad << 00), + (0x49 << 16) | (0xff << 8) | (0xad << 00), + (0x49 << 16) | (0xff << 8) | (0xac << 00), + (0x4d << 16) | (0xff << 8) | (0xaa << 00), + (0x4d << 16) | (0xff << 8) | (0xaa << 00), + (0x4d << 16) | (0xff << 8) | (0xaa << 00), + (0x4d << 16) | (0xff << 8) | (0xaa << 00), + (0x50 << 16) | (0xff << 8) | (0xa7 << 00), + (0x50 << 16) | (0xff << 8) | (0xa7 << 00), + (0x50 << 16) | (0xff << 8) | (0xa7 << 00), + (0x50 << 16) | (0xff << 8) | (0xa6 << 00), + (0x53 << 16) | (0xff << 8) | (0xa4 << 00), + (0x53 << 16) | (0xff << 8) | (0xa4 << 00), + (0x53 << 16) | (0xff << 8) | (0xa4 << 00), + (0x53 << 16) | (0xff << 8) | (0xa4 << 00), + (0x56 << 16) | (0xff << 8) | (0xa0 << 00), + (0x56 << 16) | (0xff << 8) | (0xa0 << 00), + (0x56 << 16) | (0xff << 8) | (0xa0 << 00), + (0x56 << 16) | (0xff << 8) | (0xa0 << 00), + (0x58 << 16) | (0xff << 8) | (0x9d << 00), + (0x5a << 16) | (0xff << 8) | (0x9d << 00), + (0x5a << 16) | (0xff << 8) | (0x9d << 00), + (0x5a << 16) | (0xff << 8) | (0x9d << 00), + (0x5d << 16) | (0xff << 8) | (0x9a << 00), + (0x5d << 16) | (0xff << 8) | (0x9a << 00), + (0x5d << 16) | (0xff << 8) | (0x9a << 00), + (0x5d << 16) | (0xff << 8) | (0x9a << 00), + (0x5f << 16) | (0xff << 8) | (0x97 << 00), + (0x60 << 16) | (0xff << 8) | (0x97 << 00), + (0x60 << 16) | (0xff << 8) | (0x97 << 00), + (0x60 << 16) | (0xff << 8) | (0x96 << 00), + (0x63 << 16) | (0xff << 8) | (0x94 << 00), + (0x63 << 16) | (0xff << 8) | (0x94 << 00), + (0x63 << 16) | (0xff << 8) | (0x94 << 00), + (0x63 << 16) | (0xff << 8) | (0x94 << 00), + (0x65 << 16) | (0xff << 8) | (0x90 << 00), + (0x66 << 16) | (0xff << 8) | (0x90 << 00), + (0x66 << 16) | (0xff << 8) | (0x90 << 00), + (0x66 << 16) | (0xff << 8) | (0x8f << 00), + (0x6a << 16) | (0xff << 8) | (0x8d << 00), + (0x6a << 16) | (0xff << 8) | (0x8d << 00), + (0x6a << 16) | (0xff << 8) | (0x8d << 00), + (0x6c << 16) | (0xff << 8) | (0x8a << 00), + (0x6d << 16) | (0xff << 8) | (0x8a << 00), + (0x6d << 16) | (0xff << 8) | (0x8a << 00), + (0x6d << 16) | (0xff << 8) | (0x8a << 00), + (0x70 << 16) | (0xff << 8) | (0x87 << 00), + (0x70 << 16) | (0xff << 8) | (0x87 << 00), + (0x70 << 16) | (0xff << 8) | (0x87 << 00), + (0x70 << 16) | (0xff << 8) | (0x87 << 00), + (0x72 << 16) | (0xff << 8) | (0x83 << 00), + (0x73 << 16) | (0xff << 8) | (0x83 << 00), + (0x73 << 16) | (0xff << 8) | (0x83 << 00), + (0x73 << 16) | (0xff << 8) | (0x83 << 00), + (0x77 << 16) | (0xff << 8) | (0x80 << 00), + (0x77 << 16) | (0xff << 8) | (0x80 << 00), + (0x77 << 16) | (0xff << 8) | (0x80 << 00), + (0x77 << 16) | (0xff << 8) | (0x80 << 00), + (0x79 << 16) | (0xff << 8) | (0x7d << 00), + (0x7a << 16) | (0xff << 8) | (0x7d << 00), + (0x7a << 16) | (0xff << 8) | (0x7d << 00), + (0x7a << 16) | (0xff << 8) | (0x7c << 00), + (0x7d << 16) | (0xff << 8) | (0x7a << 00), + (0x7d << 16) | (0xff << 8) | (0x7a << 00), + (0x7d << 16) | (0xff << 8) | (0x7a << 00), + (0x7d << 16) | (0xff << 8) | (0x7a << 00), + (0x80 << 16) | (0xff << 8) | (0x77 << 00), + (0x80 << 16) | (0xff << 8) | (0x77 << 00), + (0x80 << 16) | (0xff << 8) | (0x77 << 00), + (0x80 << 16) | (0xff << 8) | (0x77 << 00), + (0x81 << 16) | (0xff << 8) | (0x74 << 00), + (0x83 << 16) | (0xff << 8) | (0x73 << 00), + (0x83 << 16) | (0xff << 8) | (0x73 << 00), + (0x83 << 16) | (0xff << 8) | (0x73 << 00), + (0x87 << 16) | (0xff << 8) | (0x70 << 00), + (0x87 << 16) | (0xff << 8) | (0x70 << 00), + (0x87 << 16) | (0xff << 8) | (0x70 << 00), + (0x87 << 16) | (0xff << 8) | (0x70 << 00), + (0x89 << 16) | (0xff << 8) | (0x6d << 00), + (0x8a << 16) | (0xff << 8) | (0x6d << 00), + (0x8a << 16) | (0xff << 8) | (0x6d << 00), + (0x8a << 16) | (0xff << 8) | (0x6d << 00), + (0x8d << 16) | (0xff << 8) | (0x6a << 00), + (0x8d << 16) | (0xff << 8) | (0x6a << 00), + (0x8d << 16) | (0xff << 8) | (0x69 << 00), + (0x90 << 16) | (0xff << 8) | (0x66 << 00), + (0x90 << 16) | (0xff << 8) | (0x66 << 00), + (0x90 << 16) | (0xff << 8) | (0x66 << 00), + (0x90 << 16) | (0xff << 8) | (0x66 << 00), + (0x94 << 16) | (0xff << 8) | (0x63 << 00), + (0x94 << 16) | (0xff << 8) | (0x63 << 00), + (0x94 << 16) | (0xff << 8) | (0x63 << 00), + (0x94 << 16) | (0xff << 8) | (0x63 << 00), + (0x95 << 16) | (0xff << 8) | (0x61 << 00), + (0x97 << 16) | (0xff << 8) | (0x60 << 00), + (0x97 << 16) | (0xff << 8) | (0x60 << 00), + (0x97 << 16) | (0xff << 8) | (0x60 << 00), + (0x9a << 16) | (0xff << 8) | (0x5d << 00), + (0x9a << 16) | (0xff << 8) | (0x5d << 00), + (0x9a << 16) | (0xff << 8) | (0x5d << 00), + (0x9a << 16) | (0xff << 8) | (0x5d << 00), + (0x9c << 16) | (0xff << 8) | (0x5a << 00), + (0x9d << 16) | (0xff << 8) | (0x5a << 00), + (0x9d << 16) | (0xff << 8) | (0x5a << 00), + (0x9d << 16) | (0xff << 8) | (0x5a << 00), + (0xa0 << 16) | (0xff << 8) | (0x56 << 00), + (0xa0 << 16) | (0xff << 8) | (0x56 << 00), + (0xa0 << 16) | (0xff << 8) | (0x56 << 00), + (0xa0 << 16) | (0xff << 8) | (0x56 << 00), + (0xa3 << 16) | (0xff << 8) | (0x53 << 00), + (0xa4 << 16) | (0xff << 8) | (0x53 << 00), + (0xa4 << 16) | (0xff << 8) | (0x53 << 00), + (0xa4 << 16) | (0xff << 8) | (0x53 << 00), + (0xa7 << 16) | (0xff << 8) | (0x50 << 00), + (0xa7 << 16) | (0xff << 8) | (0x50 << 00), + (0xa7 << 16) | (0xff << 8) | (0x50 << 00), + (0xa7 << 16) | (0xff << 8) | (0x50 << 00), + (0xa9 << 16) | (0xff << 8) | (0x4d << 00), + (0xaa << 16) | (0xff << 8) | (0x4d << 00), + (0xaa << 16) | (0xff << 8) | (0x4d << 00), + (0xaa << 16) | (0xff << 8) | (0x4d << 00), + (0xab << 16) | (0xff << 8) | (0x4a << 00), + (0xad << 16) | (0xff << 8) | (0x49 << 00), + (0xad << 16) | (0xff << 8) | (0x49 << 00), + (0xaf << 16) | (0xff << 8) | (0x46 << 00), + (0xb1 << 16) | (0xff << 8) | (0x46 << 00), + (0xb1 << 16) | (0xff << 8) | (0x46 << 00), + (0xb1 << 16) | (0xff << 8) | (0x46 << 00), + (0xb4 << 16) | (0xff << 8) | (0x43 << 00), + (0xb4 << 16) | (0xff << 8) | (0x43 << 00), + (0xb4 << 16) | (0xff << 8) | (0x43 << 00), + (0xb4 << 16) | (0xff << 8) | (0x43 << 00), + (0xb6 << 16) | (0xff << 8) | (0x40 << 00), + (0xb7 << 16) | (0xff << 8) | (0x40 << 00), + (0xb7 << 16) | (0xff << 8) | (0x40 << 00), + (0xb7 << 16) | (0xff << 8) | (0x40 << 00), + (0xba << 16) | (0xff << 8) | (0x3c << 00), + (0xba << 16) | (0xff << 8) | (0x3c << 00), + (0xba << 16) | (0xff << 8) | (0x3c << 00), + (0xba << 16) | (0xff << 8) | (0x3c << 00), + (0xbd << 16) | (0xff << 8) | (0x39 << 00), + (0xbe << 16) | (0xff << 8) | (0x39 << 00), + (0xbe << 16) | (0xff << 8) | (0x39 << 00), + (0xbe << 16) | (0xff << 8) | (0x39 << 00), + (0xbf << 16) | (0xff << 8) | (0x37 << 00), + (0xc1 << 16) | (0xff << 8) | (0x36 << 00), + (0xc1 << 16) | (0xff << 8) | (0x36 << 00), + (0xc1 << 16) | (0xff << 8) | (0x36 << 00), + (0xc4 << 16) | (0xff << 8) | (0x33 << 00), + (0xc4 << 16) | (0xff << 8) | (0x33 << 00), + (0xc4 << 16) | (0xff << 8) | (0x33 << 00), + (0xc4 << 16) | (0xff << 8) | (0x33 << 00), + (0xc5 << 16) | (0xff << 8) | (0x31 << 00), + (0xc7 << 16) | (0xff << 8) | (0x30 << 00), + (0xc7 << 16) | (0xff << 8) | (0x30 << 00), + (0xc7 << 16) | (0xff << 8) | (0x30 << 00), + (0xca << 16) | (0xff << 8) | (0x2c << 00), + (0xca << 16) | (0xff << 8) | (0x2c << 00), + (0xca << 16) | (0xff << 8) | (0x2c << 00), + (0xca << 16) | (0xff << 8) | (0x2c << 00), + (0xcc << 16) | (0xff << 8) | (0x29 << 00), + (0xce << 16) | (0xff << 8) | (0x29 << 00), + (0xce << 16) | (0xff << 8) | (0x29 << 00), + (0xd0 << 16) | (0xff << 8) | (0x26 << 00), + (0xd1 << 16) | (0xff << 8) | (0x26 << 00), + (0xd1 << 16) | (0xff << 8) | (0x26 << 00), + (0xd1 << 16) | (0xff << 8) | (0x26 << 00), + (0xd2 << 16) | (0xff << 8) | (0x24 << 00), + (0xd4 << 16) | (0xff << 8) | (0x23 << 00), + (0xd4 << 16) | (0xff << 8) | (0x23 << 00), + (0xd4 << 16) | (0xff << 8) | (0x23 << 00), + (0xd7 << 16) | (0xff << 8) | (0x1f << 00), + (0xd7 << 16) | (0xff << 8) | (0x1f << 00), + (0xd7 << 16) | (0xff << 8) | (0x1f << 00), + (0xd7 << 16) | (0xff << 8) | (0x1f << 00), + (0xd9 << 16) | (0xff << 8) | (0x1d << 00), + (0xdb << 16) | (0xff << 8) | (0x1c << 00), + (0xdb << 16) | (0xff << 8) | (0x1c << 00), + (0xdb << 16) | (0xff << 8) | (0x1c << 00), + (0xde << 16) | (0xff << 8) | (0x19 << 00), + (0xde << 16) | (0xff << 8) | (0x19 << 00), + (0xde << 16) | (0xff << 8) | (0x19 << 00), + (0xde << 16) | (0xff << 8) | (0x19 << 00), + (0xe0 << 16) | (0xff << 8) | (0x00 << 00), + (0xe1 << 16) | (0xff << 8) | (0x00 << 00), + (0xe1 << 16) | (0xff << 8) | (0x00 << 00), + (0xe1 << 16) | (0xff << 8) | (0x00 << 00), + (0xe4 << 16) | (0xff << 8) | (0x13 << 00), + (0xe4 << 16) | (0xff << 8) | (0x13 << 00), + (0xe4 << 16) | (0xff << 8) | (0x13 << 00), + (0xe4 << 16) | (0xff << 8) | (0x13 << 00), + (0xe6 << 16) | (0xff << 8) | (0x0f << 00), + (0xe7 << 16) | (0xff << 8) | (0x0f << 00), + (0xe7 << 16) | (0xff << 8) | (0x0f << 00), + (0xe7 << 16) | (0xff << 8) | (0x0f << 00), + (0xeb << 16) | (0xff << 8) | (0x0c << 00), + (0xeb << 16) | (0xff << 8) | (0x0c << 00), + (0xeb << 16) | (0xff << 8) | (0x0c << 00), + (0xeb << 16) | (0xff << 8) | (0x0c << 00), + (0xed << 16) | (0xff << 8) | (0x09 << 00), + (0xee << 16) | (0xff << 8) | (0x09 << 00), + (0xee << 16) | (0xff << 8) | (0x09 << 00), + (0xf1 << 16) | (0xfc << 8) | (0x06 << 00), + (0xf1 << 16) | (0xfc << 8) | (0x06 << 00), + (0xf1 << 16) | (0xfc << 8) | (0x06 << 00), + (0xf1 << 16) | (0xfc << 8) | (0x06 << 00), + (0xf2 << 16) | (0xf9 << 8) | (0x03 << 00), + (0xf4 << 16) | (0xf8 << 8) | (0x02 << 00), + (0xf4 << 16) | (0xf8 << 8) | (0x02 << 00), + (0xf4 << 16) | (0xf8 << 8) | (0x02 << 00), + (0xf8 << 16) | (0xf5 << 8) | (0x00 << 00), + (0xf8 << 16) | (0xf5 << 8) | (0x00 << 00), + (0xf8 << 16) | (0xf5 << 8) | (0x00 << 00), + (0xf8 << 16) | (0xf5 << 8) | (0x00 << 00), + (0xfa << 16) | (0xf1 << 8) | (0x00 << 00), + (0xfb << 16) | (0xf1 << 8) | (0x00 << 00), + (0xfb << 16) | (0xf1 << 8) | (0x00 << 00), + (0xfb << 16) | (0xf1 << 8) | (0x00 << 00), + (0xfe << 16) | (0xed << 8) | (0x00 << 00), + (0xfe << 16) | (0xed << 8) | (0x00 << 00), + (0xfe << 16) | (0xed << 8) | (0x00 << 00), + (0xfe << 16) | (0xed << 8) | (0x00 << 00), + (0xfe << 16) | (0xea << 8) | (0x00 << 00), + (0xff << 16) | (0xea << 8) | (0x00 << 00), + (0xff << 16) | (0xea << 8) | (0x00 << 00), + (0xff << 16) | (0xea << 8) | (0x00 << 00), + (0xff << 16) | (0xe8 << 8) | (0x00 << 00), + (0xff << 16) | (0xe6 << 8) | (0x00 << 00), + (0xff << 16) | (0xe6 << 8) | (0x00 << 00), + (0xff << 16) | (0xe6 << 8) | (0x00 << 00), + (0xff << 16) | (0xe2 << 8) | (0x00 << 00), + (0xff << 16) | (0xe2 << 8) | (0x00 << 00), + (0xff << 16) | (0xe2 << 8) | (0x00 << 00), + (0xff << 16) | (0xe2 << 8) | (0x00 << 00), + (0xff << 16) | (0xdf << 8) | (0x00 << 00), + (0xff << 16) | (0xde << 8) | (0x00 << 00), + (0xff << 16) | (0xde << 8) | (0x00 << 00), + (0xff << 16) | (0xde << 8) | (0x00 << 00), + (0xff << 16) | (0xdb << 8) | (0x00 << 00), + (0xff << 16) | (0xdb << 8) | (0x00 << 00), + (0xff << 16) | (0xdb << 8) | (0x00 << 00), + (0xff << 16) | (0xdb << 8) | (0x00 << 00), + (0xff << 16) | (0xd8 << 8) | (0x00 << 00), + (0xff << 16) | (0xd7 << 8) | (0x00 << 00), + (0xff << 16) | (0xd7 << 8) | (0x00 << 00), + (0xff << 16) | (0xd3 << 8) | (0x00 << 00), + (0xff << 16) | (0xd3 << 8) | (0x00 << 00), + (0xff << 16) | (0xd3 << 8) | (0x00 << 00), + (0xff << 16) | (0xd3 << 8) | (0x00 << 00), + (0xff << 16) | (0xd1 << 8) | (0x00 << 00), + (0xff << 16) | (0xd0 << 8) | (0x00 << 00), + (0xff << 16) | (0xd0 << 8) | (0x00 << 00), + (0xff << 16) | (0xd0 << 8) | (0x00 << 00), + (0xff << 16) | (0xcc << 8) | (0x00 << 00), + (0xff << 16) | (0xcc << 8) | (0x00 << 00), + (0xff << 16) | (0xcc << 8) | (0x00 << 00), + (0xff << 16) | (0xcc << 8) | (0x00 << 00), + (0xff << 16) | (0xc9 << 8) | (0x00 << 00), + (0xff << 16) | (0xc8 << 8) | (0x00 << 00), + (0xff << 16) | (0xc8 << 8) | (0x00 << 00), + (0xff << 16) | (0xc8 << 8) | (0x00 << 00), + (0xff << 16) | (0xc4 << 8) | (0x00 << 00), + (0xff << 16) | (0xc4 << 8) | (0x00 << 00), + (0xff << 16) | (0xc4 << 8) | (0x00 << 00), + (0xff << 16) | (0xc4 << 8) | (0x00 << 00), + (0xff << 16) | (0xc1 << 8) | (0x00 << 00), + (0xff << 16) | (0xc1 << 8) | (0x00 << 00), + (0xff << 16) | (0xc1 << 8) | (0x00 << 00), + (0xff << 16) | (0xc1 << 8) | (0x00 << 00), + (0xff << 16) | (0xbd << 8) | (0x00 << 00), + (0xff << 16) | (0xbd << 8) | (0x00 << 00), + (0xff << 16) | (0xbd << 8) | (0x00 << 00), + (0xff << 16) | (0xbd << 8) | (0x00 << 00), + (0xff << 16) | (0xb9 << 8) | (0x00 << 00), + (0xff << 16) | (0xb9 << 8) | (0x00 << 00), + (0xff << 16) | (0xb9 << 8) | (0x00 << 00), + (0xff << 16) | (0xb9 << 8) | (0x00 << 00), + (0xff << 16) | (0xb7 << 8) | (0x00 << 00), + (0xff << 16) | (0xb6 << 8) | (0x00 << 00), + (0xff << 16) | (0xb6 << 8) | (0x00 << 00), + (0xff << 16) | (0xb6 << 8) | (0x00 << 00), + (0xff << 16) | (0xb2 << 8) | (0x00 << 00), + (0xff << 16) | (0xb2 << 8) | (0x00 << 00), + (0xff << 16) | (0xb2 << 8) | (0x00 << 00), + (0xff << 16) | (0xae << 8) | (0x00 << 00), + (0xff << 16) | (0xae << 8) | (0x00 << 00), + (0xff << 16) | (0xae << 8) | (0x00 << 00), + (0xff << 16) | (0xae << 8) | (0x00 << 00), + (0xff << 16) | (0xac << 8) | (0x00 << 00), + (0xff << 16) | (0xab << 8) | (0x00 << 00), + (0xff << 16) | (0xab << 8) | (0x00 << 00), + (0xff << 16) | (0xab << 8) | (0x00 << 00), + (0xff << 16) | (0xa7 << 8) | (0x00 << 00), + (0xff << 16) | (0xa7 << 8) | (0x00 << 00), + (0xff << 16) | (0xa7 << 8) | (0x00 << 00), + (0xff << 16) | (0xa7 << 8) | (0x00 << 00), + (0xff << 16) | (0xa3 << 8) | (0x00 << 00), + (0xff << 16) | (0xa3 << 8) | (0x00 << 00), + (0xff << 16) | (0xa3 << 8) | (0x00 << 00), + (0xff << 16) | (0xa3 << 8) | (0x00 << 00), + (0xff << 16) | (0xa1 << 8) | (0x00 << 00), + (0xff << 16) | (0x9f << 8) | (0x00 << 00), + (0xff << 16) | (0x9f << 8) | (0x00 << 00), + (0xff << 16) | (0x9f << 8) | (0x00 << 00), + (0xff << 16) | (0x9c << 8) | (0x00 << 00), + (0xff << 16) | (0x9c << 8) | (0x00 << 00), + (0xff << 16) | (0x9c << 8) | (0x00 << 00), + (0xff << 16) | (0x9c << 8) | (0x00 << 00), + (0xff << 16) | (0x9a << 8) | (0x00 << 00), + (0xff << 16) | (0x98 << 8) | (0x00 << 00), + (0xff << 16) | (0x98 << 8) | (0x00 << 00), + (0xff << 16) | (0x98 << 8) | (0x00 << 00), + (0xff << 16) | (0x94 << 8) | (0x00 << 00), + (0xff << 16) | (0x94 << 8) | (0x00 << 00), + (0xff << 16) | (0x94 << 8) | (0x00 << 00), + (0xff << 16) | (0x94 << 8) | (0x00 << 00), + (0xff << 16) | (0x92 << 8) | (0x00 << 00), + (0xff << 16) | (0x91 << 8) | (0x00 << 00), + (0xff << 16) | (0x91 << 8) | (0x00 << 00), + (0xff << 16) | (0x91 << 8) | (0x00 << 00), + (0xff << 16) | (0x8d << 8) | (0x00 << 00), + (0xff << 16) | (0x8d << 8) | (0x00 << 00), + (0xff << 16) | (0x8d << 8) | (0x00 << 00), + (0xff << 16) | (0x8b << 8) | (0x00 << 00), + (0xff << 16) | (0x89 << 8) | (0x00 << 00), + (0xff << 16) | (0x89 << 8) | (0x00 << 00), + (0xff << 16) | (0x89 << 8) | (0x00 << 00), + (0xff << 16) | (0x86 << 8) | (0x00 << 00), + (0xff << 16) | (0x86 << 8) | (0x00 << 00), + (0xff << 16) | (0x86 << 8) | (0x00 << 00), + (0xff << 16) | (0x86 << 8) | (0x00 << 00), + (0xff << 16) | (0x84 << 8) | (0x00 << 00), + (0xff << 16) | (0x82 << 8) | (0x00 << 00), + (0xff << 16) | (0x82 << 8) | (0x00 << 00), + (0xff << 16) | (0x82 << 8) | (0x00 << 00), + (0xff << 16) | (0x7e << 8) | (0x00 << 00), + (0xff << 16) | (0x7e << 8) | (0x00 << 00), + (0xff << 16) | (0x7e << 8) | (0x00 << 00), + (0xff << 16) | (0x7e << 8) | (0x00 << 00), + (0xff << 16) | (0x7b << 8) | (0x00 << 00), + (0xff << 16) | (0x7a << 8) | (0x00 << 00), + (0xff << 16) | (0x7a << 8) | (0x00 << 00), + (0xff << 16) | (0x7a << 8) | (0x00 << 00), + (0xff << 16) | (0x77 << 8) | (0x00 << 00), + (0xff << 16) | (0x77 << 8) | (0x00 << 00), + (0xff << 16) | (0x77 << 8) | (0x00 << 00), + (0xff << 16) | (0x77 << 8) | (0x00 << 00), + (0xff << 16) | (0x74 << 8) | (0x00 << 00), + (0xff << 16) | (0x73 << 8) | (0x00 << 00), + (0xff << 16) | (0x73 << 8) | (0x00 << 00), + (0xff << 16) | (0x73 << 8) | (0x00 << 00), + (0xff << 16) | (0x6f << 8) | (0x00 << 00), + (0xff << 16) | (0x6f << 8) | (0x00 << 00), + (0xff << 16) | (0x6f << 8) | (0x00 << 00), + (0xff << 16) | (0x6f << 8) | (0x00 << 00), + (0xff << 16) | (0x6c << 8) | (0x00 << 00), + (0xff << 16) | (0x6c << 8) | (0x00 << 00), + (0xff << 16) | (0x6c << 8) | (0x00 << 00), + (0xff << 16) | (0x6c << 8) | (0x00 << 00), + (0xff << 16) | (0x6a << 8) | (0x00 << 00), + (0xff << 16) | (0x68 << 8) | (0x00 << 00), + (0xff << 16) | (0x68 << 8) | (0x00 << 00), + (0xff << 16) | (0x66 << 8) | (0x00 << 00), + (0xff << 16) | (0x64 << 8) | (0x00 << 00), + (0xff << 16) | (0x64 << 8) | (0x00 << 00), + (0xff << 16) | (0x64 << 8) | (0x00 << 00), + (0xff << 16) | (0x60 << 8) | (0x00 << 00), + (0xff << 16) | (0x60 << 8) | (0x00 << 00), + (0xff << 16) | (0x60 << 8) | (0x00 << 00), + (0xff << 16) | (0x60 << 8) | (0x00 << 00), + (0xff << 16) | (0x5e << 8) | (0x00 << 00), + (0xff << 16) | (0x5d << 8) | (0x00 << 00), + (0xff << 16) | (0x5d << 8) | (0x00 << 00), + (0xff << 16) | (0x5d << 8) | (0x00 << 00), + (0xff << 16) | (0x59 << 8) | (0x00 << 00), + (0xff << 16) | (0x59 << 8) | (0x00 << 00), + (0xff << 16) | (0x59 << 8) | (0x00 << 00), + (0xff << 16) | (0x59 << 8) | (0x00 << 00), + (0xff << 16) | (0x56 << 8) | (0x00 << 00), + (0xff << 16) | (0x55 << 8) | (0x00 << 00), + (0xff << 16) | (0x55 << 8) | (0x00 << 00), + (0xff << 16) | (0x55 << 8) | (0x00 << 00), + (0xff << 16) | (0x54 << 8) | (0x00 << 00), + (0xff << 16) | (0x52 << 8) | (0x00 << 00), + (0xff << 16) | (0x52 << 8) | (0x00 << 00), + (0xff << 16) | (0x52 << 8) | (0x00 << 00), + (0xff << 16) | (0x4e << 8) | (0x00 << 00), + (0xff << 16) | (0x4e << 8) | (0x00 << 00), + (0xff << 16) | (0x4e << 8) | (0x00 << 00), + (0xff << 16) | (0x4e << 8) | (0x00 << 00), + (0xff << 16) | (0x4c << 8) | (0x00 << 00), + (0xff << 16) | (0x4a << 8) | (0x00 << 00), + (0xff << 16) | (0x4a << 8) | (0x00 << 00), + (0xff << 16) | (0x4a << 8) | (0x00 << 00), + (0xff << 16) | (0x47 << 8) | (0x00 << 00), + (0xff << 16) | (0x47 << 8) | (0x00 << 00), + (0xff << 16) | (0x47 << 8) | (0x00 << 00), + (0xff << 16) | (0x47 << 8) | (0x00 << 00), + (0xff << 16) | (0x44 << 8) | (0x00 << 00), + (0xff << 16) | (0x43 << 8) | (0x00 << 00), + (0xff << 16) | (0x43 << 8) | (0x00 << 00), + (0xff << 16) | (0x43 << 8) | (0x00 << 00), + (0xff << 16) | (0x3f << 8) | (0x00 << 00), + (0xff << 16) | (0x3f << 8) | (0x00 << 00), + (0xff << 16) | (0x3f << 8) | (0x00 << 00), + (0xff << 16) | (0x3d << 8) | (0x00 << 00), + (0xff << 16) | (0x3b << 8) | (0x00 << 00), + (0xff << 16) | (0x3b << 8) | (0x00 << 00), + (0xff << 16) | (0x3b << 8) | (0x00 << 00), + (0xff << 16) | (0x38 << 8) | (0x00 << 00), + (0xff << 16) | (0x38 << 8) | (0x00 << 00), + (0xff << 16) | (0x38 << 8) | (0x00 << 00), + (0xff << 16) | (0x38 << 8) | (0x00 << 00), + (0xff << 16) | (0x36 << 8) | (0x00 << 00), + (0xff << 16) | (0x34 << 8) | (0x00 << 00), + (0xff << 16) | (0x34 << 8) | (0x00 << 00), + (0xff << 16) | (0x34 << 8) | (0x00 << 00), + (0xff << 16) | (0x30 << 8) | (0x00 << 00), + (0xff << 16) | (0x30 << 8) | (0x00 << 00), + (0xff << 16) | (0x30 << 8) | (0x00 << 00), + (0xff << 16) | (0x30 << 8) | (0x00 << 00), + (0xff << 16) | (0x2e << 8) | (0x00 << 00), + (0xff << 16) | (0x2d << 8) | (0x00 << 00), + (0xff << 16) | (0x2d << 8) | (0x00 << 00), + (0xff << 16) | (0x2d << 8) | (0x00 << 00), + (0xff << 16) | (0x29 << 8) | (0x00 << 00), + (0xff << 16) | (0x29 << 8) | (0x00 << 00), + (0xff << 16) | (0x29 << 8) | (0x00 << 00), + (0xff << 16) | (0x29 << 8) | (0x00 << 00), + (0xff << 16) | (0x26 << 8) | (0x00 << 00), + (0xff << 16) | (0x25 << 8) | (0x00 << 00), + (0xff << 16) | (0x25 << 8) | (0x00 << 00), + (0xff << 16) | (0x25 << 8) | (0x00 << 00), + (0xff << 16) | (0x24 << 8) | (0x00 << 00), + (0xff << 16) | (0x22 << 8) | (0x00 << 00), + (0xff << 16) | (0x22 << 8) | (0x00 << 00), + (0xff << 16) | (0x22 << 8) | (0x00 << 00), + (0xff << 16) | (0x1f << 8) | (0x00 << 00), + (0xff << 16) | (0x1e << 8) | (0x00 << 00), + (0xff << 16) | (0x1e << 8) | (0x00 << 00), + (0xff << 16) | (0x1e << 8) | (0x00 << 00), + (0xff << 16) | (0x1c << 8) | (0x00 << 00), + (0xff << 16) | (0x1a << 8) | (0x00 << 00), + (0xff << 16) | (0x1a << 8) | (0x00 << 00), + (0xff << 16) | (0x18 << 8) | (0x00 << 00), + (0xff << 16) | (0x00 << 8) | (0x00 << 00), + (0xff << 16) | (0x00 << 8) | (0x00 << 00), + (0xff << 16) | (0x00 << 8) | (0x00 << 00), + (0xff << 16) | (0x13 << 8) | (0x00 << 00), + (0xff << 16) | (0x13 << 8) | (0x00 << 00), + (0xff << 16) | (0x13 << 8) | (0x00 << 00), + (0xff << 16) | (0x13 << 8) | (0x00 << 00), + (0xfc << 16) | (0x10 << 8) | (0x00 << 00), + (0xfa << 16) | (0x0f << 8) | (0x00 << 00), + (0xfa << 16) | (0x0f << 8) | (0x00 << 00), + (0xfa << 16) | (0x0f << 8) | (0x00 << 00), + (0xf9 << 16) | (0x0e << 8) | (0x00 << 00), + (0xf6 << 16) | (0x0b << 8) | (0x00 << 00), + (0xf6 << 16) | (0x0b << 8) | (0x00 << 00), + (0xf6 << 16) | (0x0b << 8) | (0x00 << 00), + (0xf2 << 16) | (0x08 << 8) | (0x00 << 00), + (0xf1 << 16) | (0x08 << 8) | (0x00 << 00), + (0xf1 << 16) | (0x08 << 8) | (0x00 << 00), + (0xf1 << 16) | (0x08 << 8) | (0x00 << 00), + (0xef << 16) | (0x06 << 8) | (0x00 << 00), + (0xed << 16) | (0x04 << 8) | (0x00 << 00), + (0xed << 16) | (0x04 << 8) | (0x00 << 00), + (0xed << 16) | (0x04 << 8) | (0x00 << 00), + (0xe8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xe8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xe8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xe8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xe6 << 16) | (0x00 << 8) | (0x00 << 00), + (0xe4 << 16) | (0x00 << 8) | (0x00 << 00), + (0xe4 << 16) | (0x00 << 8) | (0x00 << 00), + (0xe4 << 16) | (0x00 << 8) | (0x00 << 00), + (0xdf << 16) | (0x00 << 8) | (0x00 << 00), + (0xdf << 16) | (0x00 << 8) | (0x00 << 00), + (0xdf << 16) | (0x00 << 8) | (0x00 << 00), + (0xdf << 16) | (0x00 << 8) | (0x00 << 00), + (0xdc << 16) | (0x00 << 8) | (0x00 << 00), + (0xda << 16) | (0x00 << 8) | (0x00 << 00), + (0xda << 16) | (0x00 << 8) | (0x00 << 00), + (0xd7 << 16) | (0x00 << 8) | (0x00 << 00), + (0xd6 << 16) | (0x00 << 8) | (0x00 << 00), + (0xd6 << 16) | (0x00 << 8) | (0x00 << 00), + (0xd6 << 16) | (0x00 << 8) | (0x00 << 00), + (0xd4 << 16) | (0x00 << 8) | (0x00 << 00), + (0xd1 << 16) | (0x00 << 8) | (0x00 << 00), + (0xd1 << 16) | (0x00 << 8) | (0x00 << 00), + (0xd1 << 16) | (0x00 << 8) | (0x00 << 00), + (0xcd << 16) | (0x00 << 8) | (0x00 << 00), + (0xcd << 16) | (0x00 << 8) | (0x00 << 00), + (0xcd << 16) | (0x00 << 8) | (0x00 << 00), + (0xcd << 16) | (0x00 << 8) | (0x00 << 00), + (0xcb << 16) | (0x00 << 8) | (0x00 << 00), + (0xc8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xc8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xc8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xc4 << 16) | (0x00 << 8) | (0x00 << 00), + (0xc4 << 16) | (0x00 << 8) | (0x00 << 00), + (0xc4 << 16) | (0x00 << 8) | (0x00 << 00), + (0xc4 << 16) | (0x00 << 8) | (0x00 << 00), + (0xc1 << 16) | (0x00 << 8) | (0x00 << 00), + (0xbf << 16) | (0x00 << 8) | (0x00 << 00), + (0xbf << 16) | (0x00 << 8) | (0x00 << 00), + (0xbf << 16) | (0x00 << 8) | (0x00 << 00), + (0xbb << 16) | (0x00 << 8) | (0x00 << 00), + (0xbb << 16) | (0x00 << 8) | (0x00 << 00), + (0xbb << 16) | (0x00 << 8) | (0x00 << 00), + (0xbb << 16) | (0x00 << 8) | (0x00 << 00), + (0xb7 << 16) | (0x00 << 8) | (0x00 << 00), + (0xb6 << 16) | (0x00 << 8) | (0x00 << 00), + (0xb6 << 16) | (0x00 << 8) | (0x00 << 00), + (0xb6 << 16) | (0x00 << 8) | (0x00 << 00), + (0xb5 << 16) | (0x00 << 8) | (0x00 << 00), + (0xb2 << 16) | (0x00 << 8) | (0x00 << 00), + (0xb2 << 16) | (0x00 << 8) | (0x00 << 00), + (0xb2 << 16) | (0x00 << 8) | (0x00 << 00), + (0xae << 16) | (0x00 << 8) | (0x00 << 00), + (0xad << 16) | (0x00 << 8) | (0x00 << 00), + (0xad << 16) | (0x00 << 8) | (0x00 << 00), + (0xa8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xa8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xa8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xa8 << 16) | (0x00 << 8) | (0x00 << 00), + (0xa6 << 16) | (0x00 << 8) | (0x00 << 00), + (0xa4 << 16) | (0x00 << 8) | (0x00 << 00), + (0xa4 << 16) | (0x00 << 8) | (0x00 << 00), + (0xa4 << 16) | (0x00 << 8) | (0x00 << 00), + (0x9f << 16) | (0x00 << 8) | (0x00 << 00), + (0x9f << 16) | (0x00 << 8) | (0x00 << 00), + (0x9f << 16) | (0x00 << 8) | (0x00 << 00), + (0x9f << 16) | (0x00 << 8) | (0x00 << 00), + (0x9c << 16) | (0x00 << 8) | (0x00 << 00), + (0x9b << 16) | (0x00 << 8) | (0x00 << 00), + (0x9b << 16) | (0x00 << 8) | (0x00 << 00), + (0x9b << 16) | (0x00 << 8) | (0x00 << 00), + (0x9a << 16) | (0x00 << 8) | (0x00 << 00), + (0x96 << 16) | (0x00 << 8) | (0x00 << 00), + (0x96 << 16) | (0x00 << 8) | (0x00 << 00), + (0x96 << 16) | (0x00 << 8) | (0x00 << 00), + (0x93 << 16) | (0x00 << 8) | (0x00 << 00), + (0x92 << 16) | (0x00 << 8) | (0x00 << 00), + (0x92 << 16) | (0x00 << 8) | (0x00 << 00), + (0x92 << 16) | (0x00 << 8) | (0x00 << 00), + (0x90 << 16) | (0x00 << 8) | (0x00 << 00), + (0x8d << 16) | (0x00 << 8) | (0x00 << 00), + (0x8d << 16) | (0x00 << 8) | (0x00 << 00), + (0x8d << 16) | (0x00 << 8) | (0x00 << 00), + (0x89 << 16) | (0x00 << 8) | (0x00 << 00), + (0x89 << 16) | (0x00 << 8) | (0x00 << 00), + (0x89 << 16) | (0x00 << 8) | (0x00 << 00), + (0x89 << 16) | (0x00 << 8) | (0x00 << 00), + (0x87 << 16) | (0x00 << 8) | (0x00 << 00), + (0x84 << 16) | (0x00 << 8) | (0x00 << 00), + (0x84 << 16) | (0x00 << 8) | (0x00 << 00), + (0x84 << 16) | (0x00 << 8) | (0x00 << 00), + (0x80 << 16) | (0x00 << 8) | (0x00 << 00), + (0x80 << 16) | (0x00 << 8) | (0x00 << 00), + (0x80 << 16) | (0x00 << 8) | (0x00 << 00), (0x7d << 16) | (0x00 << 8) | (0x00 << 00) }; - -#endif +#endif // PEOPLE_TRACKING_FILTER_RGB_H diff --git a/people_tracking_filter/include/people_tracking_filter/state_pos_vel.h b/people_tracking_filter/include/people_tracking_filter/state_pos_vel.h index b2ba8fe3..f1240ed2 100644 --- a/people_tracking_filter/include/people_tracking_filter/state_pos_vel.h +++ b/people_tracking_filter/include/people_tracking_filter/state_pos_vel.h @@ -35,8 +35,8 @@ /* Author: Wim Meeussen */ -#ifndef STATE_POS_VEL_H -#define STATE_POS_VEL_H +#ifndef PEOPLE_TRACKING_FILTER_STATE_POS_VEL_H +#define PEOPLE_TRACKING_FILTER_STATE_POS_VEL_H #include @@ -50,10 +50,10 @@ class StatePosVel /// Constructor StatePosVel(const tf::Vector3& pos = tf::Vector3(0, 0, 0), - const tf::Vector3& vel = tf::Vector3(0, 0, 0)): pos_(pos), vel_(vel) {}; + const tf::Vector3& vel = tf::Vector3(0, 0, 0)): pos_(pos), vel_(vel) {} /// Destructor - ~StatePosVel() {}; + ~StatePosVel() {} /// operator += StatePosVel& operator += (const StatePosVel& s) @@ -80,10 +80,6 @@ class StatePosVel << "(" << s.vel_[0] << ", " << s.vel_[1] << ", " << s.vel_[2] << ") "; return os; }; - - - - }; -} // end namespace -#endif +} // end namespace BFL +#endif // PEOPLE_TRACKING_FILTER_STATE_POS_VEL_H diff --git a/people_tracking_filter/include/people_tracking_filter/sysmodel_pos_vel.h b/people_tracking_filter/include/people_tracking_filter/sysmodel_pos_vel.h index ff00f36f..cc629870 100644 --- a/people_tracking_filter/include/people_tracking_filter/sysmodel_pos_vel.h +++ b/people_tracking_filter/include/people_tracking_filter/sysmodel_pos_vel.h @@ -34,12 +34,12 @@ /* Author: Wim Meeussen */ -#ifndef SYSMODEL_POS_VEL_H -#define SYSMODEL_POS_VEL_H +#ifndef PEOPLE_TRACKING_FILTER_SYSMODEL_POS_VEL_H +#define PEOPLE_TRACKING_FILTER_SYSMODEL_POS_VEL_H -#include "state_pos_vel.h" -#include "gaussian_pos_vel.h" +#include +#include #include #include #include @@ -47,13 +47,12 @@ namespace BFL { - class SysPdfPosVel : public ConditionalPdf { public: /// Constructor - SysPdfPosVel(const StatePosVel& sigma); + explicit SysPdfPosVel(const StatePosVel& sigma); /// Destructor virtual ~SysPdfPosVel(); @@ -62,52 +61,38 @@ class SysPdfPosVel void SetDt(double dt) { dt_ = dt; - }; + } // Redefining pure virtual methods virtual bool SampleFrom(BFL::Sample& one_sample, int method, void *args) const; - virtual StatePosVel ExpectedValueGet() const; // not applicable - virtual Probability ProbabilityGet(const StatePosVel& state) const; // not applicable - virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable - + virtual StatePosVel ExpectedValueGet() const; // not applicable + virtual Probability ProbabilityGet(const StatePosVel& state) const; // not applicable + virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable private: GaussianPosVel noise_; double dt_; - -}; // class - - - - - - +}; // class class SysModelPosVel : public SystemModel { public: - SysModelPosVel(const StatePosVel& sigma) + explicit SysModelPosVel(const StatePosVel& sigma) : SystemModel(new SysPdfPosVel(sigma)) - {}; + {} /// destructor ~SysModelPosVel() { delete SystemPdfGet(); - }; + } // set time void SetDt(double dt) { - ((SysPdfPosVel*)SystemPdfGet())->SetDt(dt); - }; - -}; // class - - - -} //namespace - - -#endif + static_cast(SystemPdfGet())->SetDt(dt); + } +}; // class +} // namespace BFL +#endif // PEOPLE_TRACKING_FILTER_SYSMODEL_POS_VEL_H diff --git a/people_tracking_filter/include/people_tracking_filter/sysmodel_vector.h b/people_tracking_filter/include/people_tracking_filter/sysmodel_vector.h index 89a1d4c4..55d5f6ce 100644 --- a/people_tracking_filter/include/people_tracking_filter/sysmodel_vector.h +++ b/people_tracking_filter/include/people_tracking_filter/sysmodel_vector.h @@ -34,11 +34,11 @@ /* Author: Wim Meeussen */ -#ifndef SYSMODEL_VECTOR_H -#define SYSMODEL_VECTOR_H +#ifndef PEOPLE_TRACKING_FILTER_SYSMODEL_VECTOR_H +#define PEOPLE_TRACKING_FILTER_SYSMODEL_VECTOR_H -#include "gaussian_vector.h" +#include #include #include #include @@ -52,7 +52,7 @@ class SysPdfVector { public: /// Constructor - SysPdfVector(const tf::Vector3& sigma); + explicit SysPdfVector(const tf::Vector3& sigma); /// Destructor virtual ~SysPdfVector(); @@ -61,52 +61,38 @@ class SysPdfVector void SetDt(double dt) { dt_ = dt; - }; + } // Redefining pure virtual methods virtual bool SampleFrom(BFL::Sample& one_sample, int method, void *args) const; - virtual tf::Vector3 ExpectedValueGet() const; // not applicable - virtual Probability ProbabilityGet(const tf::Vector3& state) const; // not applicable - virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable - + virtual tf::Vector3 ExpectedValueGet() const; // not applicable + virtual Probability ProbabilityGet(const tf::Vector3& state) const; // not applicable + virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; // Not applicable private: GaussianVector noise_; double dt_; - -}; // class - - - - - - +}; // class class SysModelVector : public SystemModel { public: - SysModelVector(const tf::Vector3& sigma) + explicit SysModelVector(const tf::Vector3& sigma) : SystemModel(new SysPdfVector(sigma)) - {}; + {} /// destructor ~SysModelVector() { delete SystemPdfGet(); - }; + } // set time void SetDt(double dt) { - ((SysPdfVector*)SystemPdfGet())->SetDt(dt); - }; - -}; // class - - - -} //namespace - - -#endif + static_cast(SystemPdfGet())->SetDt(dt); + } +}; // class +} // namespace BFL +#endif // PEOPLE_TRACKING_FILTER_SYSMODEL_VECTOR_H diff --git a/people_tracking_filter/include/people_tracking_filter/tracker.h b/people_tracking_filter/include/people_tracking_filter/tracker.h index ff0320d3..e9f3016a 100644 --- a/people_tracking_filter/include/people_tracking_filter/tracker.h +++ b/people_tracking_filter/include/people_tracking_filter/tracker.h @@ -34,10 +34,10 @@ /* Author: Wim Meeussen */ -#ifndef __TRACKER__ -#define __TRACKER__ +#ifndef PEOPLE_TRACKING_FILTER_TRACKER_H +#define PEOPLE_TRACKING_FILTER_TRACKER_H -#include "state_pos_vel.h" +#include #include #include #include @@ -50,10 +50,10 @@ class Tracker { public: /// constructor - Tracker(const std::string& name): name_(name) {}; + explicit Tracker(const std::string& name): name_(name) {} /// destructor - virtual ~Tracker() {}; + virtual ~Tracker() {} /// return the name of the tracker const std::string& getName() const @@ -87,9 +87,6 @@ class Tracker private: std::string name_; - -}; // class - -}; // namespace - -#endif +}; // class +} // namespace estimation +#endif // PEOPLE_TRACKING_FILTER_TRACKER_H diff --git a/people_tracking_filter/include/people_tracking_filter/tracker_kalman.h b/people_tracking_filter/include/people_tracking_filter/tracker_kalman.h index e6185556..86dcb442 100644 --- a/people_tracking_filter/include/people_tracking_filter/tracker_kalman.h +++ b/people_tracking_filter/include/people_tracking_filter/tracker_kalman.h @@ -34,10 +34,10 @@ /* Author: Wim Meeussen */ -#ifndef __TRACKER_KALMAN__ -#define __TRACKER_KALMAN__ +#ifndef PEOPLE_TRACKING_FILTER_TRACKER_KALMAN_H +#define PEOPLE_TRACKING_FILTER_TRACKER_KALMAN_H -#include "tracker.h" +#include // bayesian filtering #include @@ -46,13 +46,14 @@ #include -#include "state_pos_vel.h" +#include // TF #include // log files #include +#include namespace estimation { @@ -96,7 +97,6 @@ class TrackerKalman: public Tracker virtual void getEstimate(BFL::StatePosVel& est) const; virtual void getEstimate(people_msgs::PositionMeasurement& est) const; - private: // pdf / model / filter BFL::Gaussian prior_; @@ -113,10 +113,6 @@ class TrackerKalman: public Tracker // vars bool tracker_initialized_; double init_time_, filter_time_, quality_; - - -}; // class - -}; // namespace - -#endif +}; // class +} // namespace estimation +#endif // PEOPLE_TRACKING_FILTER_TRACKER_KALMAN_H diff --git a/people_tracking_filter/include/people_tracking_filter/tracker_particle.h b/people_tracking_filter/include/people_tracking_filter/tracker_particle.h index 240f2aec..2794f557 100644 --- a/people_tracking_filter/include/people_tracking_filter/tracker_particle.h +++ b/people_tracking_filter/include/people_tracking_filter/tracker_particle.h @@ -34,17 +34,17 @@ /* Author: Wim Meeussen */ -#ifndef __TRACKER_PARTICLE__ -#define __TRACKER_PARTICLE__ +#ifndef PEOPLE_TRACKING_FILTER_TRACKER_PARTICLE_H +#define PEOPLE_TRACKING_FILTER_TRACKER_PARTICLE_H -#include "tracker.h" +#include // bayesian filtering #include -#include "state_pos_vel.h" -#include "mcpdf_pos_vel.h" -#include "sysmodel_pos_vel.h" -#include "measmodel_pos.h" +#include +#include +#include +#include // TF #include @@ -54,10 +54,10 @@ // log files #include +#include namespace estimation { - class TrackerParticle: public Tracker { public: @@ -74,13 +74,13 @@ class TrackerParticle: public Tracker virtual bool isInitialized() const { return tracker_initialized_; - }; + } /// return measure for tracker quality: 0=bad 1=good virtual double getQuality() const { return quality_; - }; + } /// return the lifetime of the tracker virtual double getLifetime() const; @@ -115,10 +115,6 @@ class TrackerParticle: public Tracker bool tracker_initialized_; double init_time_, filter_time_, quality_; unsigned int num_particles_; - - -}; // class - -}; // namespace - -#endif +}; // class +} // namespace estimation +#endif // PEOPLE_TRACKING_FILTER_TRACKER_PARTICLE_H diff --git a/people_tracking_filter/include/people_tracking_filter/uniform_vector.h b/people_tracking_filter/include/people_tracking_filter/uniform_vector.h index c311a2a7..356d3e73 100644 --- a/people_tracking_filter/include/people_tracking_filter/uniform_vector.h +++ b/people_tracking_filter/include/people_tracking_filter/uniform_vector.h @@ -34,13 +34,12 @@ /* Author: Wim Meeussen */ -#ifndef UNIFORM_VECTOR_H -#define UNIFORM_VECTOR_H +#ifndef PEOPLE_TRACKING_FILTER_UNIFORM_VECTOR_H +#define PEOPLE_TRACKING_FILTER_UNIFORM_VECTOR_H #include #include - - +#include namespace BFL { @@ -66,13 +65,12 @@ class UniformVector: public Pdf // Redefinition of pure virtuals virtual Probability ProbabilityGet(const tf::Vector3& input) const; - bool SampleFrom(vector >& list_samples, const int num_samples, int method = DEFAULT, void * args = NULL) const; + bool SampleFrom(vector >& list_samples, const int num_samples, int method = DEFAULT, + void * args = NULL) const; virtual bool SampleFrom(Sample& one_sample, int method = DEFAULT, void * args = NULL) const; virtual tf::Vector3 ExpectedValueGet() const; virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const; - }; - -} // end namespace -#endif +} // namespace BFL +#endif // PEOPLE_TRACKING_FILTER_UNIFORM_VECTOR_H diff --git a/people_tracking_filter/launch/filter.launch b/people_tracking_filter/launch/filter.launch index 6181677f..4f63d1ad 100644 --- a/people_tracking_filter/launch/filter.launch +++ b/people_tracking_filter/launch/filter.launch @@ -1,26 +1,25 @@ - - - - - - +\n\n + + + + + + - - + + - - - - - - - - - - + + + + + + + + diff --git a/people_tracking_filter/package.xml b/people_tracking_filter/package.xml index 8af0b005..523f2578 100644 --- a/people_tracking_filter/package.xml +++ b/people_tracking_filter/package.xml @@ -19,4 +19,6 @@ sensor_msgs std_msgs tf + roslaunch + roslint diff --git a/people_tracking_filter/src/detector_particle.cpp b/people_tracking_filter/src/detector_particle.cpp index 8eb1116f..efb1c5f3 100644 --- a/people_tracking_filter/src/detector_particle.cpp +++ b/people_tracking_filter/src/detector_particle.cpp @@ -34,21 +34,14 @@ /* Author: Wim Meeussen */ -#include "people_tracking_filter/detector_particle.h" -#include "people_tracking_filter/uniform_vector.h" - -using namespace MatrixWrapper; -using namespace BFL; -using namespace tf; -using namespace std; -using namespace ros; -using namespace geometry_msgs; - - - +#include +#include +#include namespace estimation { +using MatrixWrapper::Matrix; + // constructor DetectorParticle::DetectorParticle(unsigned int num_particles): prior_(num_particles), @@ -59,26 +52,23 @@ DetectorParticle::DetectorParticle(unsigned int num_particles): num_particles_(num_particles) {} - - // destructor DetectorParticle::~DetectorParticle() { if (filter_) delete filter_; } - // initialize prior density of filter void DetectorParticle::initialize(const tf::Vector3& mu, const tf::Vector3& size, const double time) { - cout << "Initializing detector with " << num_particles_ << " particles, with uniform size " - << size << " around " << mu << endl; + std::cout << "Initializing detector with " << num_particles_ << " particles, with uniform size " + << size << " around " << mu << std::endl; - UniformVector uniform_vector(mu, size); - vector > prior_samples(num_particles_); + BFL::UniformVector uniform_vector(mu, size); + std::vector > prior_samples(num_particles_); uniform_vector.SampleFrom(prior_samples, num_particles_, CHOLESKY, NULL); prior_.ListOfSamplesSet(prior_samples); - filter_ = new BootstrapFilter(&prior_, &prior_, 0, num_particles_ / 4.0); + filter_ = new BFL::BootstrapFilter(&prior_, &prior_, 0, num_particles_ / 4.0); // detector initialized detector_initialized_ = true; @@ -86,9 +76,6 @@ void DetectorParticle::initialize(const tf::Vector3& mu, const tf::Vector3& size filter_time_ = time; } - - - // update filter prediction bool DetectorParticle::updatePrediction(const double dt) { @@ -102,10 +89,9 @@ bool DetectorParticle::updatePrediction(const double dt) return res; } - - // update filter correction -bool DetectorParticle::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov, const double time) +bool DetectorParticle::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov, + const double time) { assert(cov.columns() == 3); @@ -113,7 +99,7 @@ bool DetectorParticle::updateCorrection(const tf::Vector3& meas, const MatrixWr filter_time_ = time; // set covariance - ((MeasPdfVector*)(meas_model_.MeasurementPdfGet()))->CovarianceSet(cov); + static_cast(meas_model_.MeasurementPdfGet())->CovarianceSet(cov); // update filter bool res = filter_->Update(&meas_model_, meas); @@ -122,21 +108,18 @@ bool DetectorParticle::updateCorrection(const tf::Vector3& meas, const MatrixWr return res; } - // get evenly spaced particle cloud void DetectorParticle::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const { - ((MCPdfVector*)(filter_->PostGet()))->getParticleCloud(step, threshold, cloud); + static_cast(filter_->PostGet())->getParticleCloud(step, threshold, cloud); } - // get most recent filter posterior void DetectorParticle::getEstimate(tf::Vector3& est) const { - est = ((MCPdfVector*)(filter_->PostGet()))->ExpectedValueGet(); + est = static_cast(filter_->PostGet())->ExpectedValueGet(); } - void DetectorParticle::getEstimate(people_msgs::PositionMeasurement& est) const { tf::Vector3 tmp = filter_->PostGet()->ExpectedValueGet(); @@ -149,18 +132,9 @@ void DetectorParticle::getEstimate(people_msgs::PositionMeasurement& est) const est.header.frame_id = "base_link"; } - - - - /// Get histogram from certain area Matrix DetectorParticle::getHistogram(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const { - return ((MCPdfVector*)(filter_->PostGet()))->getHistogram(min, max, step); + return static_cast(filter_->PostGet())->getHistogram(min, max, step); } - - -}; // namespace - - - +} // namespace estimation diff --git a/people_tracking_filter/src/gaussian_pos_vel.cpp b/people_tracking_filter/src/gaussian_pos_vel.cpp index 5253a671..3cd38814 100644 --- a/people_tracking_filter/src/gaussian_pos_vel.cpp +++ b/people_tracking_filter/src/gaussian_pos_vel.cpp @@ -34,13 +34,11 @@ /* Author: Wim Meeussen */ - -#include "people_tracking_filter/gaussian_pos_vel.h" +#include #include #include #include - -using namespace tf; +#include namespace BFL { @@ -52,7 +50,6 @@ GaussianPosVel::GaussianPosVel(const StatePosVel& mu, const StatePosVel& sigma) gauss_vel_(mu.vel_, sigma.vel_) {} - GaussianPosVel::~GaussianPosVel() {} GaussianPosVel* GaussianPosVel::Clone() const @@ -62,44 +59,41 @@ GaussianPosVel* GaussianPosVel::Clone() const std::ostream& operator<< (std::ostream& os, const GaussianPosVel& g) { - os << "\nMu pos :\n" << g.ExpectedValueGet().pos_ << endl - << "\nMu vel :\n" << g.ExpectedValueGet().vel_ << endl - << "\nSigma:\n" << g.CovarianceGet() << endl; + os << "\nMu pos :\n" << g.ExpectedValueGet().pos_ << std::endl + << "\nMu vel :\n" << g.ExpectedValueGet().vel_ << std::endl + << "\nSigma:\n" << g.CovarianceGet() << std::endl; return os; } - Probability GaussianPosVel::ProbabilityGet(const StatePosVel& input) const { return gauss_pos_.ProbabilityGet(input.pos_) * gauss_vel_.ProbabilityGet(input.vel_); } - bool -GaussianPosVel::SampleFrom(vector >& list_samples, const int num_samples, int method, void * args) const +GaussianPosVel::SampleFrom(std::vector >& list_samples, const int num_samples, int method, + void * args) const { list_samples.resize(num_samples); - vector >::iterator sample_it = list_samples.begin(); + std::vector >::iterator sample_it = list_samples.begin(); for (sample_it = list_samples.begin(); sample_it != list_samples.end(); sample_it++) SampleFrom(*sample_it, method, args); return true; } - bool GaussianPosVel::SampleFrom(Sample& one_sample, int method, void * args) const { - one_sample.ValueSet(StatePosVel(Vector3(rnorm(mu_.pos_[0], sigma_.pos_[0]*dt_), - rnorm(mu_.pos_[1], sigma_.pos_[1]*dt_), - rnorm(mu_.pos_[2], sigma_.pos_[2]*dt_)), - Vector3(rnorm(mu_.vel_[0], sigma_.vel_[0]*dt_), - rnorm(mu_.vel_[1], sigma_.vel_[1]*dt_), - rnorm(mu_.vel_[2], sigma_.vel_[2]*dt_)))); + one_sample.ValueSet(StatePosVel(tf::Vector3(rnorm(mu_.pos_[0], sigma_.pos_[0]*dt_), + rnorm(mu_.pos_[1], sigma_.pos_[1]*dt_), + rnorm(mu_.pos_[2], sigma_.pos_[2]*dt_)), + tf::Vector3(rnorm(mu_.vel_[0], sigma_.vel_[0]*dt_), + rnorm(mu_.vel_[1], sigma_.vel_[1]*dt_), + rnorm(mu_.vel_[2], sigma_.vel_[2]*dt_)))); return true; } - StatePosVel GaussianPosVel::ExpectedValueGet() const { @@ -118,5 +112,4 @@ GaussianPosVel::CovarianceGet() const } return sigma; } - -} // End namespace BFL +} // End namespace BFL diff --git a/people_tracking_filter/src/gaussian_vector.cpp b/people_tracking_filter/src/gaussian_vector.cpp index f3cae134..cc8f6d47 100644 --- a/people_tracking_filter/src/gaussian_vector.cpp +++ b/people_tracking_filter/src/gaussian_vector.cpp @@ -34,17 +34,16 @@ /* Author: Wim Meeussen */ -#include "people_tracking_filter/gaussian_vector.h" +#include #include #include #include - -using namespace tf; +#include namespace BFL { -GaussianVector::GaussianVector(const Vector3& mu, const Vector3& sigma) - : Pdf (1), +GaussianVector::GaussianVector(const tf::Vector3& mu, const tf::Vector3& sigma) + : Pdf (1), mu_(mu), sigma_(sigma), sigma_changed_(true) @@ -53,24 +52,22 @@ GaussianVector::GaussianVector(const Vector3& mu, const Vector3& sigma) assert(sigma[i] > 0); } - GaussianVector::~GaussianVector() {} - std::ostream& operator<< (std::ostream& os, const GaussianVector& g) { - os << "Mu :\n" << g.ExpectedValueGet() << endl - << "Sigma:\n" << g.CovarianceGet() << endl; + os << "Mu :\n" << g.ExpectedValueGet() << std::endl + << "Sigma:\n" << g.CovarianceGet() << std::endl; return os; } -void GaussianVector::sigmaSet(const Vector3& sigma) +void GaussianVector::sigmaSet(const tf::Vector3& sigma) { sigma_ = sigma; sigma_changed_ = true; } -Probability GaussianVector::ProbabilityGet(const Vector3& input) const +Probability GaussianVector::ProbabilityGet(const tf::Vector3& input) const { if (sigma_changed_) { @@ -82,36 +79,34 @@ Probability GaussianVector::ProbabilityGet(const Vector3& input) const sqrt_ = 1 / sqrt(M_PI * M_PI * M_PI * sigma_sq_[0] * sigma_sq_[1] * sigma_sq_[2]); } - Vector3 diff = input - mu_; + tf::Vector3 diff = input - mu_; return sqrt_ * exp(- (diff[0] * diff[0] / sigma_sq_[0]) - (diff[1] * diff[1] / sigma_sq_[1]) - (diff[2] * diff[2] / sigma_sq_[2])); } - bool -GaussianVector::SampleFrom(vector >& list_samples, const int num_samples, int method, void * args) const +GaussianVector::SampleFrom(std::vector >& list_samples, const int num_samples, int method, + void * args) const { list_samples.resize(num_samples); - vector >::iterator sample_it = list_samples.begin(); + std::vector >::iterator sample_it = list_samples.begin(); for (sample_it = list_samples.begin(); sample_it != list_samples.end(); sample_it++) SampleFrom(*sample_it, method, args); return true; } - bool -GaussianVector::SampleFrom(Sample& one_sample, int method, void * args) const +GaussianVector::SampleFrom(Sample& one_sample, int method, void * args) const { - one_sample.ValueSet(Vector3(rnorm(mu_[0], sigma_[0]), - rnorm(mu_[1], sigma_[1]), - rnorm(mu_[2], sigma_[2]))); + one_sample.ValueSet(tf::Vector3(rnorm(mu_[0], sigma_[0]), + rnorm(mu_[1], sigma_[1]), + rnorm(mu_[2], sigma_[2]))); return true; } - -Vector3 +tf::Vector3 GaussianVector::ExpectedValueGet() const { return mu_; @@ -132,5 +127,4 @@ GaussianVector::Clone() const { return new GaussianVector(mu_, sigma_); } - -} // End namespace BFL +} // End namespace BFL diff --git a/people_tracking_filter/src/mcpdf_pos_vel.cpp b/people_tracking_filter/src/mcpdf_pos_vel.cpp index 1615f5f4..1537e941 100644 --- a/people_tracking_filter/src/mcpdf_pos_vel.cpp +++ b/people_tracking_filter/src/mcpdf_pos_vel.cpp @@ -34,35 +34,30 @@ /* Author: Wim Meeussen */ -#include "people_tracking_filter/mcpdf_pos_vel.h" +#include #include #include #include -#include "people_tracking_filter/rgb.h" - - -using namespace MatrixWrapper; -using namespace BFL; -using namespace tf; +#include +#include +namespace BFL +{ static const unsigned int NUM_CONDARG = 1; - MCPdfPosVel::MCPdfPosVel(unsigned int num_samples) : MCPdf (num_samples, NUM_CONDARG) {} MCPdfPosVel::~MCPdfPosVel() {} - WeightedSample MCPdfPosVel::SampleGet(unsigned int particle) const { - assert((int)particle >= 0 && particle < _listOfSamples.size()); + assert(static_cast(particle) >= 0 && particle < _listOfSamples.size()); return _listOfSamples[particle]; } - StatePosVel MCPdfPosVel::ExpectedValueGet() const { tf::Vector3 pos(0, 0, 0); @@ -78,19 +73,18 @@ StatePosVel MCPdfPosVel::ExpectedValueGet() const return StatePosVel(pos, vel); } - /// Get evenly distributed particle cloud void MCPdfPosVel::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const { unsigned int num_samples = _listOfSamples.size(); assert(num_samples > 0); - Vector3 m = _listOfSamples[0].ValueGet().pos_; - Vector3 M = _listOfSamples[0].ValueGet().pos_; + tf::Vector3 m = _listOfSamples[0].ValueGet().pos_; + tf::Vector3 M = _listOfSamples[0].ValueGet().pos_; // calculate min and max for (unsigned int s = 0; s < num_samples; s++) { - Vector3 v = _listOfSamples[s].ValueGet().pos_; + tf::Vector3 v = _listOfSamples[s].ValueGet().pos_; for (unsigned int i = 0; i < 3; i++) { if (v[i] < m[i]) m[i] = v[i]; @@ -108,8 +102,8 @@ void MCPdfPosVel::getParticleCloud(const tf::Vector3& step, double threshold, se for (unsigned int c = 1; c <= col; c++) if (hist(r, c) > threshold) total++; - vector points(total); - vector weights(total); + std::vector points(total); + std::vector weights(total); sensor_msgs::ChannelFloat32 channel; for (unsigned int r = 1; r <= row; r++) for (unsigned int c = 1; c <= col; c++) @@ -119,7 +113,7 @@ void MCPdfPosVel::getParticleCloud(const tf::Vector3& step, double threshold, se points[t].x = m[0] + (step[0] * r); points[t].y = m[1] + (step[1] * c); points[t].z = m[2]; - weights[t] = rgb[999 - (int)trunc(max(0.0, min(999.0, hist(r, c) * 2 * total * total)))]; + weights[t] = rgb[999 - static_cast(trunc(std::max(0.0, std::min(999.0, hist(r, c) * 2 * total * total))))]; t++; } cloud.header.frame_id = "odom_combined"; @@ -129,23 +123,25 @@ void MCPdfPosVel::getParticleCloud(const tf::Vector3& step, double threshold, se cloud.channels.push_back(channel); } - /// Get histogram from pos -MatrixWrapper::Matrix MCPdfPosVel::getHistogramPos(const Vector3& m, const Vector3& M, const Vector3& step) const +MatrixWrapper::Matrix MCPdfPosVel::getHistogramPos(const tf::Vector3& m, + const tf::Vector3& M, + const tf::Vector3& step) const { return getHistogram(m, M, step, true); } - /// Get histogram from vel -MatrixWrapper::Matrix MCPdfPosVel::getHistogramVel(const Vector3& m, const Vector3& M, const Vector3& step) const +MatrixWrapper::Matrix MCPdfPosVel::getHistogramVel(const tf::Vector3& m, + const tf::Vector3& M, + const tf::Vector3& step) const { return getHistogram(m, M, step, false); } - /// Get histogram from certain area -MatrixWrapper::Matrix MCPdfPosVel::getHistogram(const Vector3& m, const Vector3& M, const Vector3& step, bool pos_hist) const +MatrixWrapper::Matrix MCPdfPosVel::getHistogram(const tf::Vector3& m, const tf::Vector3& M, const tf::Vector3& step, + bool pos_hist) const { unsigned int num_samples = _listOfSamples.size(); unsigned int rows = round((M[0] - m[0]) / step[0]); @@ -156,7 +152,7 @@ MatrixWrapper::Matrix MCPdfPosVel::getHistogram(const Vector3& m, const Vector3& // calculate histogram for (unsigned int i = 0; i < num_samples; i++) { - Vector3 rel; + tf::Vector3 rel; if (pos_hist) rel = _listOfSamples[i].ValueGet().pos_ - m; else @@ -171,12 +167,9 @@ MatrixWrapper::Matrix MCPdfPosVel::getHistogram(const Vector3& m, const Vector3& return hist; } - - unsigned int MCPdfPosVel::numParticlesGet() const { return _listOfSamples.size(); } - - +} // namespace BFL diff --git a/people_tracking_filter/src/mcpdf_vector.cpp b/people_tracking_filter/src/mcpdf_vector.cpp index 054a1e75..db7b4e99 100644 --- a/people_tracking_filter/src/mcpdf_vector.cpp +++ b/people_tracking_filter/src/mcpdf_vector.cpp @@ -34,63 +34,57 @@ /* Author: Wim Meeussen */ -#include "people_tracking_filter/mcpdf_vector.h" -#include "people_tracking_filter/state_pos_vel.h" +#include +#include #include #include #include -#include "people_tracking_filter/rgb.h" - - -using namespace MatrixWrapper; -using namespace BFL; -using namespace tf; +#include +#include +namespace BFL +{ static const unsigned int NUM_CONDARG = 1; - MCPdfVector::MCPdfVector(unsigned int num_samples) - : MCPdf (num_samples, NUM_CONDARG) + : MCPdf (num_samples, NUM_CONDARG) {} MCPdfVector::~MCPdfVector() {} - -WeightedSample +WeightedSample MCPdfVector::SampleGet(unsigned int particle) const { - assert((int)particle >= 0 && particle < _listOfSamples.size()); + assert(static_cast(particle) >= 0 && particle < _listOfSamples.size()); return _listOfSamples[particle]; } - -Vector3 MCPdfVector::ExpectedValueGet() const +tf::Vector3 MCPdfVector::ExpectedValueGet() const { - Vector3 pos(0, 0, 0); + tf::Vector3 pos(0, 0, 0); double current_weight; - std::vector >::const_iterator it_los; + std::vector >::const_iterator it_los; for (it_los = _listOfSamples.begin() ; it_los != _listOfSamples.end() ; it_los++) { current_weight = it_los->WeightGet(); pos += (it_los->ValueGet() * current_weight); } - return Vector3(pos); + return tf::Vector3(pos); } - /// Get evenly distributed particle cloud -void MCPdfVector::getParticleCloud(const Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const +void MCPdfVector::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const { unsigned int num_samples = _listOfSamples.size(); assert(num_samples > 0); - Vector3 m = _listOfSamples[0].ValueGet(); - Vector3 M = _listOfSamples[0].ValueGet(); + tf::Vector3 m = _listOfSamples[0].ValueGet(); + tf::Vector3 M = _listOfSamples[0].ValueGet(); // calculate min and max for (unsigned int s = 0; s < num_samples; s++) { - Vector3 v = _listOfSamples[s].ValueGet(); + tf::Vector3 v = _listOfSamples[s].ValueGet(); for (unsigned int i = 0; i < 3; i++) { if (v[i] < m[i]) m[i] = v[i]; @@ -107,10 +101,10 @@ void MCPdfVector::getParticleCloud(const Vector3& step, double threshold, sensor for (unsigned int r = 1; r <= row; r++) for (unsigned int c = 1; c <= col; c++) if (hist(r, c) > threshold) total++; - cout << "size total " << total << endl; + std::cout << "size total " << total << std::endl; - vector points(total); - vector weights(total); + std::vector points(total); + std::vector weights(total); sensor_msgs::ChannelFloat32 channel; for (unsigned int r = 1; r <= row; r++) for (unsigned int c = 1; c <= col; c++) @@ -120,10 +114,10 @@ void MCPdfVector::getParticleCloud(const Vector3& step, double threshold, sensor points[t].x = m[0] + (step[0] * r); points[t].y = m[1] + (step[1] * c); points[t].z = m[2]; - weights[t] = rgb[999 - (int)trunc(max(0.0, min(999.0, hist(r, c) * 2 * total * total)))]; + weights[t] = rgb[999 - static_cast(trunc(std::max(0.0, std::min(999.0, hist(r, c) * 2 * total * total))))]; t++; } - cout << "points size " << points.size() << endl; + std::cout << "points size " << points.size() << std::endl; cloud.header.frame_id = "base_link"; cloud.points = points; channel.name = "rgb"; @@ -131,9 +125,10 @@ void MCPdfVector::getParticleCloud(const Vector3& step, double threshold, sensor cloud.channels.push_back(channel); } - /// Get histogram from pos -MatrixWrapper::Matrix MCPdfVector::getHistogram(const Vector3& m, const Vector3& M, const Vector3& step) const +MatrixWrapper::Matrix MCPdfVector::getHistogram(const tf::Vector3& m, + const tf::Vector3& M, + const tf::Vector3& step) const { unsigned int num_samples = _listOfSamples.size(); unsigned int rows = round((M[0] - m[0]) / step[0]); @@ -144,7 +139,7 @@ MatrixWrapper::Matrix MCPdfVector::getHistogram(const Vector3& m, const Vector3& // calculate histogram for (unsigned int i = 0; i < num_samples; i++) { - Vector3 rel(_listOfSamples[i].ValueGet() - m); + tf::Vector3 rel(_listOfSamples[i].ValueGet() - m); unsigned int r = round(rel[0] / step[0]); unsigned int c = round(rel[1] / step[1]); if (r >= 1 && c >= 1 && r <= rows && c <= cols) @@ -154,12 +149,9 @@ MatrixWrapper::Matrix MCPdfVector::getHistogram(const Vector3& m, const Vector3& return hist; } - - unsigned int MCPdfVector::numParticlesGet() const { return _listOfSamples.size(); } - - +} // namespace BFL diff --git a/people_tracking_filter/src/measmodel_pos.cpp b/people_tracking_filter/src/measmodel_pos.cpp index 2b7e03d9..7930e606 100644 --- a/people_tracking_filter/src/measmodel_pos.cpp +++ b/people_tracking_filter/src/measmodel_pos.cpp @@ -34,78 +34,59 @@ /* Author: Wim Meeussen */ -#include "people_tracking_filter/measmodel_pos.h" - -using namespace std; -using namespace BFL; -using namespace tf; - +#include +namespace BFL +{ static const unsigned int NUM_MEASMODEL_POS_COND_ARGS = 1; static const unsigned int DIM_MEASMODEL_POS = 13; - - // Constructor -MeasPdfPos::MeasPdfPos(const Vector3& sigma) - : ConditionalPdf(DIM_MEASMODEL_POS, NUM_MEASMODEL_POS_COND_ARGS), - meas_noise_(Vector3(0, 0, 0), sigma) +MeasPdfPos::MeasPdfPos(const tf::Vector3& sigma) + : ConditionalPdf(DIM_MEASMODEL_POS, NUM_MEASMODEL_POS_COND_ARGS), + meas_noise_(tf::Vector3(0, 0, 0), sigma) {} - // Destructor MeasPdfPos::~MeasPdfPos() {} - - Probability -MeasPdfPos::ProbabilityGet(const Vector3& measurement) const +MeasPdfPos::ProbabilityGet(const tf::Vector3& measurement) const { return meas_noise_.ProbabilityGet(measurement - ConditionalArgumentGet(0).pos_); } - - bool -MeasPdfPos::SampleFrom(Sample& one_sample, int method, void *args) const +MeasPdfPos::SampleFrom(Sample& one_sample, int method, void *args) const { - cerr << "MeasPdfPos::SampleFrom Method not applicable" << endl; + std::cerr << "MeasPdfPos::SampleFrom Method not applicable" << std::endl; assert(0); return false; } - - - -Vector3 +tf::Vector3 MeasPdfPos::ExpectedValueGet() const { - cerr << "MeasPdfPos::ExpectedValueGet Method not applicable" << endl; - Vector3 result; + std::cerr << "MeasPdfPos::ExpectedValueGet Method not applicable" << std::endl; + tf::Vector3 result; assert(0); return result; } - - - SymmetricMatrix MeasPdfPos::CovarianceGet() const { - cerr << "MeasPdfPos::CovarianceGet Method not applicable" << endl; + std::cerr << "MeasPdfPos::CovarianceGet Method not applicable" << std::endl; SymmetricMatrix Covar(DIM_MEASMODEL_POS); assert(0); return Covar; } - void MeasPdfPos::CovarianceSet(const MatrixWrapper::SymmetricMatrix& cov) { tf::Vector3 cov_vec(sqrt(cov(1, 1)), sqrt(cov(2, 2)), sqrt(cov(3, 3))); meas_noise_.sigmaSet(cov_vec); } - - - +} // namespace BFL diff --git a/people_tracking_filter/src/measmodel_vector.cpp b/people_tracking_filter/src/measmodel_vector.cpp index 60920226..4611661d 100644 --- a/people_tracking_filter/src/measmodel_vector.cpp +++ b/people_tracking_filter/src/measmodel_vector.cpp @@ -34,76 +34,59 @@ /* Author: Wim Meeussen */ -#include "people_tracking_filter/measmodel_vector.h" - -using namespace std; -using namespace BFL; -using namespace tf; +#include +namespace BFL +{ static const unsigned int NUM_MEASMODEL_VECTOR_COND_ARGS = 1; static const unsigned int DIM_MEASMODEL_VECTOR = 3; - // Constructor -MeasPdfVector::MeasPdfVector(const Vector3& sigma) - : ConditionalPdf(DIM_MEASMODEL_VECTOR, NUM_MEASMODEL_VECTOR_COND_ARGS), - meas_noise_(Vector3(0, 0, 0), sigma) +MeasPdfVector::MeasPdfVector(const tf::Vector3& sigma) + : ConditionalPdf(DIM_MEASMODEL_VECTOR, NUM_MEASMODEL_VECTOR_COND_ARGS), + meas_noise_(tf::Vector3(0, 0, 0), sigma) {} - // Destructor MeasPdfVector::~MeasPdfVector() {} - - Probability -MeasPdfVector::ProbabilityGet(const Vector3& measurement) const +MeasPdfVector::ProbabilityGet(const tf::Vector3& measurement) const { return meas_noise_.ProbabilityGet(measurement - ConditionalArgumentGet(0)); } - - bool -MeasPdfVector::SampleFrom(Sample& one_sample, int method, void *args) const +MeasPdfVector::SampleFrom(Sample& one_sample, int method, void *args) const { - cerr << "MeasPdfVector::SampleFrom Method not applicable" << endl; + std::cerr << "MeasPdfVector::SampleFrom Method not applicable" << std::endl; assert(0); return false; } - - - -Vector3 +tf::Vector3 MeasPdfVector::ExpectedValueGet() const { - cerr << "MeasPdfVector::ExpectedValueGet Method not applicable" << endl; - Vector3 result; + std::cerr << "MeasPdfVector::ExpectedValueGet Method not applicable" << std::endl; + tf::Vector3 result; assert(0); return result; } - - - SymmetricMatrix MeasPdfVector::CovarianceGet() const { - cerr << "MeasPdfVector::CovarianceGet Method not applicable" << endl; + std::cerr << "MeasPdfVector::CovarianceGet Method not applicable" << std::endl; SymmetricMatrix Covar(DIM_MEASMODEL_VECTOR); assert(0); return Covar; } - void MeasPdfVector::CovarianceSet(const MatrixWrapper::SymmetricMatrix& cov) { - Vector3 cov_vec(sqrt(cov(1, 1)), sqrt(cov(2, 2)), sqrt(cov(3, 3))); + tf::Vector3 cov_vec(sqrt(cov(1, 1)), sqrt(cov(2, 2)), sqrt(cov(3, 3))); meas_noise_.sigmaSet(cov_vec); } - - - +} // namespace BFL diff --git a/people_tracking_filter/src/people_tracking_node.cpp b/people_tracking_filter/src/people_tracking_node.cpp index 1c87af2b..134d8045 100644 --- a/people_tracking_filter/src/people_tracking_node.cpp +++ b/people_tracking_filter/src/people_tracking_node.cpp @@ -34,20 +34,18 @@ /* Author: Wim Meeussen */ -#include "people_tracking_filter/people_tracking_node.h" -#include "people_tracking_filter/tracker_particle.h" -#include "people_tracking_filter/tracker_kalman.h" -#include "people_tracking_filter/state_pos_vel.h" -#include "people_tracking_filter/rgb.h" +#include +#include +#include +#include +#include #include +#include +#include +#include +#include - -using namespace std; -using namespace tf; -using namespace BFL; -using namespace message_filters; - -static const double sequencer_delay = 0.8; //TODO: this is probably too big, it was 0.8 +static const double sequencer_delay = 0.8; // TODO(noidea): this is probably too big, it was 0.8 static const unsigned int sequencer_internal_buffer = 100; static const unsigned int sequencer_subscribe_buffer = 10; static const unsigned int num_particles_tracker = 1000; @@ -62,14 +60,14 @@ PeopleTrackingNode::PeopleTrackingNode(ros::NodeHandle nh) tracker_counter_(0) { // initialize - meas_cloud_.points = vector(1); + meas_cloud_.points = std::vector(1); meas_cloud_.points[0].x = 0; meas_cloud_.points[0].y = 0; meas_cloud_.points[0].z = 0; // get parameters ros::NodeHandle local_nh("~"); - local_nh.param("fixed_frame", fixed_frame_, string("default")); + local_nh.param("fixed_frame", fixed_frame_, std::string("default")); local_nh.param("freq", freq_, 1.0); local_nh.param("start_distance_min", start_distance_min_, 0.0); local_nh.param("reliability_threshold", reliability_threshold_, 1.0); @@ -90,10 +88,8 @@ PeopleTrackingNode::PeopleTrackingNode(ros::NodeHandle nh) // register message sequencer people_meas_sub_ = nh_.subscribe("people_tracker_measurements", 1, &PeopleTrackingNode::callbackRcv, this); - } - // destructor PeopleTrackingNode::~PeopleTrackingNode() { @@ -101,20 +97,17 @@ PeopleTrackingNode::~PeopleTrackingNode() delete message_sequencer_; // delete all trackers - for (list::iterator it = trackers_.begin(); it != trackers_.end(); it++) + for (std::list::iterator it = trackers_.begin(); it != trackers_.end(); it++) delete *it; }; - - - // callback for messages void PeopleTrackingNode::callbackRcv(const people_msgs::PositionMeasurement::ConstPtr& message) { ROS_DEBUG("Tracking node got a people position measurement (%f,%f,%f)", message->pos.x, message->pos.y, message->pos.z); // get measurement in fixed frame - Stamped meas_rel, meas; + tf::Stamped meas_rel, meas; meas_rel.setData( tf::Vector3(message->pos.x, message->pos.y, message->pos.z)); meas_rel.stamp_ = message->header.stamp; @@ -122,7 +115,7 @@ void PeopleTrackingNode::callbackRcv(const people_msgs::PositionMeasurement::Con robot_state_.transformPoint(fixed_frame_, meas_rel, meas); // get measurement covariance - SymmetricMatrix cov(3); + BFL::SymmetricMatrix cov(3); for (unsigned int i = 0; i < 3; i++) for (unsigned int j = 0; j < 3; j++) cov(i + 1, j + 1) = message->covariance[3 * i + j]; @@ -131,7 +124,7 @@ void PeopleTrackingNode::callbackRcv(const people_msgs::PositionMeasurement::Con boost::mutex::scoped_lock lock(filter_mutex_); // update tracker if matching tracker found - for (list::iterator it = trackers_.begin(); it != trackers_.end(); it++) + for (std::list::iterator it = trackers_.begin(); it != trackers_.end(); it++) if ((*it)->getName() == message->object_id) { (*it)->updatePrediction(message->header.stamp.toSec()); @@ -141,8 +134,8 @@ void PeopleTrackingNode::callbackRcv(const people_msgs::PositionMeasurement::Con if (message->object_id == "" && message->reliability > reliability_threshold_) { double closest_tracker_dist = start_distance_min_; - StatePosVel est; - for (list::iterator it = trackers_.begin(); it != trackers_.end(); it++) + BFL::StatePosVel est; + for (std::list::iterator it = trackers_.begin(); it != trackers_.end(); it++) { (*it)->getEstimate(est); double dst = sqrt(pow(est.pos_[0] - meas[0], 2) + pow(est.pos_[1] - meas[1], 2)); @@ -151,11 +144,13 @@ void PeopleTrackingNode::callbackRcv(const people_msgs::PositionMeasurement::Con } // initialize a new tracker if (follow_one_person_) - cout << "Following one person" << endl; - if (message->initialization == 1 && ((!follow_one_person_ && (closest_tracker_dist >= start_distance_min_)) || (follow_one_person_ && trackers_.empty()))) + std::cout << "Following one person" << std::endl; + if (message->initialization == 1 && + ((!follow_one_person_ && (closest_tracker_dist >= start_distance_min_)) || + (follow_one_person_ && trackers_.empty()))) { - //if (closest_tracker_dist >= start_distance_min_ || message->initialization == 1){ - //if (message->initialization == 1 && trackers_.empty()){ + // if (closest_tracker_dist >= start_distance_min_ || message->initialization == 1){ + // if (message->initialization == 1 && trackers_.empty()){ ROS_INFO("Passed crazy conditional."); tf::Point pt; tf::pointMsgToTF(message->pos, pt); @@ -164,22 +159,24 @@ void PeopleTrackingNode::callbackRcv(const people_msgs::PositionMeasurement::Con float cur_dist; if ((cur_dist = pow(loc[0], 2.0) + pow(loc[1], 2.0)) < tracker_init_dist) { - - cout << "starting new tracker" << endl; - stringstream tracker_name; - StatePosVel prior_sigma(tf::Vector3(sqrt(cov(1, 1)), sqrt(cov( - 2, 2)), sqrt(cov(3, 3))), tf::Vector3(0.0000001, 0.0000001, 0.0000001)); + std::cout << "starting new tracker" << std::endl; + std::stringstream tracker_name; + BFL::StatePosVel prior_sigma(tf::Vector3(sqrt(cov(1, 1)), + sqrt(cov(2, 2)), + sqrt(cov(3, 3))), + tf::Vector3(0.0000001, 0.0000001, 0.0000001)); tracker_name << "person " << tracker_counter_++; Tracker* new_tracker = new TrackerKalman(tracker_name.str(), sys_sigma_); - //Tracker* new_tracker = new TrackerParticle(tracker_name.str(), num_particles_tracker, sys_sigma_); + // Tracker* new_tracker = new TrackerParticle(tracker_name.str(), num_particles_tracker, sys_sigma_); new_tracker->initialize(meas, prior_sigma, message->header.stamp.toSec()); trackers_.push_back(new_tracker); ROS_INFO("Initialized new tracker %s", tracker_name.str().c_str()); } else - ROS_INFO("Found a person, but he/she is not close enough to start following. Person is %f away, and must be less than %f away.", cur_dist , tracker_init_dist); + ROS_INFO("Found a person, but he/she is not close enough to start following. " + "Person is %f away, and must be less than %f away.", cur_dist, tracker_init_dist); } else ROS_INFO("Failed crazy conditional."); @@ -187,7 +184,6 @@ void PeopleTrackingNode::callbackRcv(const people_msgs::PositionMeasurement::Con lock.unlock(); // ------ LOCKED ------ - // visualize measurement meas_cloud_.points[0].x = meas[0]; meas_cloud_.points[0].y = meas[1]; @@ -196,19 +192,13 @@ void PeopleTrackingNode::callbackRcv(const people_msgs::PositionMeasurement::Con people_tracker_vis_pub_.publish(meas_cloud_); } - - // callback for dropped messages void PeopleTrackingNode::callbackDrop(const people_msgs::PositionMeasurement::ConstPtr& message) { ROS_INFO("DROPPED PACKAGE for %s from %s with delay %f !!!!!!!!!!!", message->object_id.c_str(), message->name.c_str(), (ros::Time::now() - message->header.stamp).toSec()); - } - - - // filter loop void PeopleTrackingNode::spin() { @@ -220,13 +210,13 @@ void PeopleTrackingNode::spin() boost::mutex::scoped_lock lock(filter_mutex_); // visualization variables - vector filter_visualize(trackers_.size()); - vector weights(trackers_.size()); + std::vector filter_visualize(trackers_.size()); + std::vector weights(trackers_.size()); sensor_msgs::ChannelFloat32 channel; // loop over trackers unsigned int i = 0; - list::iterator it = trackers_.begin(); + std::list::iterator it = trackers_.begin(); while (it != trackers_.end()) { // update prediction up to delayed time @@ -244,7 +234,13 @@ void PeopleTrackingNode::spin() filter_visualize[i].x = est_pos.pos.x; filter_visualize[i].y = est_pos.pos.y; filter_visualize[i].z = est_pos.pos.z; - weights[i] = *(float*) & (rgb[min(998, 999 - max(1, (int)trunc((*it)->getQuality() * 999.0)))]); + + // calculate weight + { + int quality = static_cast(trunc((*it)->getQuality() * 999.0)); + int index = std::min(998, 999 - std::max(1, quality)); + weights[i] = *(float*) & (rgb[index]); // NOLINT(readability/casting) + } // remove trackers that have zero quality ROS_INFO("Quality of tracker %s = %f", (*it)->getName().c_str(), (*it)->getQuality()); @@ -260,7 +256,6 @@ void PeopleTrackingNode::spin() lock.unlock(); // ------ LOCKED ------ - // visualize all trackers channel.name = "rgb"; channel.values = weights; @@ -276,19 +271,11 @@ void PeopleTrackingNode::spin() ros::spinOnce(); } }; - - -}; // namespace - - - - - +} // namespace estimation // ---------- // -- MAIN -- // ---------- -using namespace estimation; int main(int argc, char **argv) { // Initialize ROS @@ -296,12 +283,10 @@ int main(int argc, char **argv) ros::NodeHandle(nh); // create tracker node - PeopleTrackingNode my_tracking_node(nh); + estimation::PeopleTrackingNode my_tracking_node(nh); // wait for filter to finish my_tracking_node.spin(); - // Clean up - return 0; } diff --git a/people_tracking_filter/src/sysmodel_pos_vel.cpp b/people_tracking_filter/src/sysmodel_pos_vel.cpp index 3246aba2..a93e9b17 100644 --- a/people_tracking_filter/src/sysmodel_pos_vel.cpp +++ b/people_tracking_filter/src/sysmodel_pos_vel.cpp @@ -34,42 +34,32 @@ /* Author: Wim Meeussen */ +#include -#include "people_tracking_filter/sysmodel_pos_vel.h" - - -using namespace std; -using namespace BFL; -using namespace tf; - +namespace BFL +{ static const unsigned int NUM_SYS_POS_VEL_COND_ARGS = 1; static const unsigned int DIM_SYS_POS_VEL = 6; - // Constructor -SysPdfPosVel::SysPdfPosVel(const StatePosVel& sigma) +SysPdfPosVel::SysPdfPosVel(const BFL::StatePosVel& sigma) : ConditionalPdf(DIM_SYS_POS_VEL, NUM_SYS_POS_VEL_COND_ARGS), - noise_(StatePosVel(Vector3(0, 0, 0), Vector3(0, 0, 0)), sigma) + noise_(StatePosVel(tf::Vector3(0, 0, 0), tf::Vector3(0, 0, 0)), sigma) {} - - // Destructor SysPdfPosVel::~SysPdfPosVel() {} - - Probability SysPdfPosVel::ProbabilityGet(const StatePosVel& state) const { - cerr << "SysPdfPosVel::ProbabilityGet Method not applicable" << endl; + std::cerr << "SysPdfPosVel::ProbabilityGet Method not applicable" << std::endl; assert(0); return 0; } - bool SysPdfPosVel::SampleFrom(Sample& one_sample, int method, void *args) const { @@ -90,22 +80,20 @@ SysPdfPosVel::SampleFrom(Sample& one_sample, int method, void *args return true; } - StatePosVel SysPdfPosVel::ExpectedValueGet() const { - cerr << "SysPdfPosVel::ExpectedValueGet Method not applicable" << endl; + std::cerr << "SysPdfPosVel::ExpectedValueGet Method not applicable" << std::endl; assert(0); return StatePosVel(); - } SymmetricMatrix SysPdfPosVel::CovarianceGet() const { - cerr << "SysPdfPosVel::CovarianceGet Method not applicable" << endl; + std::cerr << "SysPdfPosVel::CovarianceGet Method not applicable" << std::endl; SymmetricMatrix Covar(DIM_SYS_POS_VEL); assert(0); return Covar; } - +} // namespace BFL diff --git a/people_tracking_filter/src/sysmodel_vector.cpp b/people_tracking_filter/src/sysmodel_vector.cpp index bbf49f2a..e4d6edbb 100644 --- a/people_tracking_filter/src/sysmodel_vector.cpp +++ b/people_tracking_filter/src/sysmodel_vector.cpp @@ -34,74 +34,61 @@ /* Author: Wim Meeussen */ - -#include "people_tracking_filter/sysmodel_vector.h" - - -using namespace std; -using namespace BFL; -using namespace tf; - +#include static const unsigned int NUM_SYS_VECTOR_COND_ARGS = 1; static const unsigned int DIM_SYS_VECTOR = 3; - +namespace BFL +{ // Constructor -SysPdfVector::SysPdfVector(const Vector3& sigma) - : ConditionalPdf(DIM_SYS_VECTOR, NUM_SYS_VECTOR_COND_ARGS), - noise_(Vector3(0, 0, 0), sigma) +SysPdfVector::SysPdfVector(const tf::Vector3& sigma) + : ConditionalPdf(DIM_SYS_VECTOR, NUM_SYS_VECTOR_COND_ARGS), + noise_(tf::Vector3(0, 0, 0), sigma) {} - - // Destructor SysPdfVector::~SysPdfVector() {} - - Probability -SysPdfVector::ProbabilityGet(const Vector3& state) const +SysPdfVector::ProbabilityGet(const tf::Vector3& state) const { - cerr << "SysPdfVector::ProbabilityGet Method not applicable" << endl; + std::cerr << "SysPdfVector::ProbabilityGet Method not applicable" << std::endl; assert(0); return 0; } - bool -SysPdfVector::SampleFrom(Sample& one_sample, int method, void *args) const +SysPdfVector::SampleFrom(Sample& one_sample, int method, void *args) const { - Vector3& res = one_sample.ValueGet(); + tf::Vector3& res = one_sample.ValueGet(); // get conditional argument: state res = this->ConditionalArgumentGet(0); // add noise - Sample noise_sample; + Sample noise_sample; noise_.SampleFrom(noise_sample, method, args); res += noise_sample.ValueGet(); return true; } - -Vector3 +tf::Vector3 SysPdfVector::ExpectedValueGet() const { - cerr << "SysPdfVector::ExpectedValueGet Method not applicable" << endl; + std::cerr << "SysPdfVector::ExpectedValueGet Method not applicable" << std::endl; assert(0); - return Vector3(); - + return tf::Vector3(); } SymmetricMatrix SysPdfVector::CovarianceGet() const { - cerr << "SysPdfVector::CovarianceGet Method not applicable" << endl; + std::cerr << "SysPdfVector::CovarianceGet Method not applicable" << std::endl; SymmetricMatrix Covar(DIM_SYS_VECTOR); assert(0); return Covar; } - +} // namespace BFL diff --git a/people_tracking_filter/src/tracker_kalman.cpp b/people_tracking_filter/src/tracker_kalman.cpp index 9a626f18..063388f6 100644 --- a/people_tracking_filter/src/tracker_kalman.cpp +++ b/people_tracking_filter/src/tracker_kalman.cpp @@ -34,22 +34,21 @@ /* Author: Wim Meeussen */ -#include "people_tracking_filter/tracker_kalman.h" - -using namespace MatrixWrapper; -using namespace BFL; -using namespace tf; -using namespace std; -using namespace ros; - - -const static double damping_velocity = 0.9; +#include +#include +#include +static const double damping_velocity = 0.9; namespace estimation { + +using MatrixWrapper::Matrix; +using MatrixWrapper::ColumnVector; +using MatrixWrapper::SymmetricMatrix; + // constructor -TrackerKalman::TrackerKalman(const string& name, const StatePosVel& sysnoise): +TrackerKalman::TrackerKalman(const std::string& name, const BFL::StatePosVel& sysnoise): Tracker(name), filter_(NULL), sys_pdf_(NULL), @@ -75,10 +74,9 @@ TrackerKalman::TrackerKalman(const string& name, const StatePosVel& sysnoise): sys_sigma_(i + 1, i + 1) = pow(sysnoise.pos_[i], 2); sys_sigma_(i + 4, i + 4) = pow(sysnoise.vel_[i], 2); } - Gaussian sys_noise(sys_mu, sys_sigma_); - sys_pdf_ = new LinearAnalyticConditionalGaussian(sys_matrix_, sys_noise); - sys_model_ = new LinearAnalyticSystemModelGaussianUncertainty(sys_pdf_); - + BFL::Gaussian sys_noise(sys_mu, sys_sigma_); + sys_pdf_ = new BFL::LinearAnalyticConditionalGaussian(sys_matrix_, sys_noise); + sys_model_ = new BFL::LinearAnalyticSystemModelGaussianUncertainty(sys_pdf_); // create meas model Matrix meas_matrix(3, 6); @@ -92,13 +90,11 @@ TrackerKalman::TrackerKalman(const string& name, const StatePosVel& sysnoise): meas_sigma = 0; for (unsigned int i = 0; i < 3; i++) meas_sigma(i + 1, i + 1) = 0; - Gaussian meas_noise(meas_mu, meas_sigma); - meas_pdf_ = new LinearAnalyticConditionalGaussian(meas_matrix, meas_noise); - meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_); + BFL::Gaussian meas_noise(meas_mu, meas_sigma); + meas_pdf_ = new BFL::LinearAnalyticConditionalGaussian(meas_matrix, meas_noise); + meas_model_ = new BFL::LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_); }; - - // destructor TrackerKalman::~TrackerKalman() { @@ -109,10 +105,8 @@ TrackerKalman::~TrackerKalman() if (meas_model_) delete meas_model_; }; - - // initialize prior density of filter -void TrackerKalman::initialize(const StatePosVel& mu, const StatePosVel& sigma, const double time) +void TrackerKalman::initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time) { ColumnVector mu_vec(6); SymmetricMatrix sigma_vec(6); @@ -124,8 +118,8 @@ void TrackerKalman::initialize(const StatePosVel& mu, const StatePosVel& sigma, sigma_vec(i + 1, i + 1) = pow(sigma.pos_[i], 2); sigma_vec(i + 4, i + 4) = pow(sigma.vel_[i], 2); } - prior_ = Gaussian(mu_vec, sigma_vec); - filter_ = new ExtendedKalmanFilter(&prior_); + prior_ = BFL::Gaussian(mu_vec, sigma_vec); + filter_ = new BFL::ExtendedKalmanFilter(&prior_); // tracker initialized tracker_initialized_ = true; @@ -134,9 +128,6 @@ void TrackerKalman::initialize(const StatePosVel& mu, const StatePosVel& sigma, init_time_ = time; } - - - // update filter prediction bool TrackerKalman::updatePrediction(const double time) { @@ -160,8 +151,6 @@ bool TrackerKalman::updatePrediction(const double time) return res; }; - - // update filter correction bool TrackerKalman::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov) { @@ -173,7 +162,7 @@ bool TrackerKalman::updateCorrection(const tf::Vector3& meas, const MatrixWrapp meas_vec(i + 1) = meas[i]; // set covariance - ((LinearAnalyticConditionalGaussian*)(meas_model_->MeasurementPdfGet()))->AdditiveNoiseSigmaSet(cov); + static_cast(meas_model_->MeasurementPdfGet())->AdditiveNoiseSigmaSet(cov); // update filter bool res = filter_->Update(meas_model_, meas_vec); @@ -183,8 +172,7 @@ bool TrackerKalman::updateCorrection(const tf::Vector3& meas, const MatrixWrapp return res; }; - -void TrackerKalman::getEstimate(StatePosVel& est) const +void TrackerKalman::getEstimate(BFL::StatePosVel& est) const { ColumnVector tmp = filter_->PostGet()->ExpectedValueGet(); for (unsigned int i = 0; i < 3; i++) @@ -194,7 +182,6 @@ void TrackerKalman::getEstimate(StatePosVel& est) const } }; - void TrackerKalman::getEstimate(people_msgs::PositionMeasurement& est) const { ColumnVector tmp = filter_->PostGet()->ExpectedValueGet(); @@ -207,20 +194,16 @@ void TrackerKalman::getEstimate(people_msgs::PositionMeasurement& est) const est.object_id = getName(); } - - - double TrackerKalman::calculateQuality() { double sigma_max = 0; SymmetricMatrix cov = filter_->PostGet()->CovarianceGet(); for (unsigned int i = 1; i <= 2; i++) - sigma_max = max(sigma_max, sqrt(cov(i, i))); + sigma_max = std::max(sigma_max, sqrt(cov(i, i))); - return 1.0 - min(1.0, sigma_max / 1.5); + return 1.0 - std::min(1.0, sigma_max / 1.5); } - double TrackerKalman::getLifetime() const { if (tracker_initialized_) @@ -236,7 +219,4 @@ double TrackerKalman::getTime() const else return 0; } - -}; // namespace - - +} // namespace estimation diff --git a/people_tracking_filter/src/tracker_particle.cpp b/people_tracking_filter/src/tracker_particle.cpp index a0cb140a..7b8cc845 100644 --- a/people_tracking_filter/src/tracker_particle.cpp +++ b/people_tracking_filter/src/tracker_particle.cpp @@ -34,22 +34,17 @@ /* Author: Wim Meeussen */ -#include "people_tracking_filter/tracker_particle.h" -#include "people_tracking_filter/gaussian_pos_vel.h" - -using namespace MatrixWrapper; -using namespace BFL; -using namespace tf; -using namespace std; -using namespace ros; - - - +#include +#include +#include +#include namespace estimation { +using MatrixWrapper::Matrix; + // constructor -TrackerParticle::TrackerParticle(const string& name, unsigned int num_particles, const StatePosVel& sysnoise): +TrackerParticle::TrackerParticle(const std::string& name, unsigned int num_particles, const BFL::StatePosVel& sysnoise): Tracker(name), prior_(num_particles), filter_(NULL), @@ -57,29 +52,25 @@ TrackerParticle::TrackerParticle(const string& name, unsigned int num_particles, meas_model_(tf::Vector3(0.1, 0.1, 0.1)), tracker_initialized_(false), num_particles_(num_particles) -{}; - - +{} // destructor TrackerParticle::~TrackerParticle() { if (filter_) delete filter_; -}; - +} // initialize prior density of filter -void TrackerParticle::initialize(const StatePosVel& mu, const StatePosVel& sigma, const double time) +void TrackerParticle::initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time) { - cout << "Initializing tracker with " << num_particles_ << " particles, with covariance " - << sigma << " around " << mu << endl; + std::cout << "Initializing tracker with " << num_particles_ << " particles, with covariance " + << sigma << " around " << mu << std::endl; - - GaussianPosVel gauss_pos_vel(mu, sigma); - vector > prior_samples(num_particles_); + BFL::GaussianPosVel gauss_pos_vel(mu, sigma); + std::vector > prior_samples(num_particles_); gauss_pos_vel.SampleFrom(prior_samples, num_particles_, CHOLESKY, NULL); prior_.ListOfSamplesSet(prior_samples); - filter_ = new BootstrapFilter(&prior_, &prior_, 0, num_particles_ / 4.0); + filter_ = new BFL::BootstrapFilter(&prior_, &prior_, 0, num_particles_ / 4.0); // tracker initialized tracker_initialized_ = true; @@ -88,9 +79,6 @@ void TrackerParticle::initialize(const StatePosVel& mu, const StatePosVel& sigma init_time_ = time; } - - - // update filter prediction bool TrackerParticle::updatePrediction(const double time) { @@ -108,15 +96,13 @@ bool TrackerParticle::updatePrediction(const double time) return res; }; - - // update filter correction bool TrackerParticle::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov) { assert(cov.columns() == 3); // set covariance - ((MeasPdfPos*)(meas_model_.MeasurementPdfGet()))->CovarianceSet(cov); + static_cast(meas_model_.MeasurementPdfGet())->CovarianceSet(cov); // update filter bool res = filter_->Update(&meas_model_, meas); @@ -125,24 +111,21 @@ bool TrackerParticle::updateCorrection(const tf::Vector3& meas, const MatrixWra return res; }; - // get evenly spaced particle cloud void TrackerParticle::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const { - ((MCPdfPosVel*)(filter_->PostGet()))->getParticleCloud(step, threshold, cloud); + static_cast(filter_->PostGet())->getParticleCloud(step, threshold, cloud); }; - // get most recent filter posterior -void TrackerParticle::getEstimate(StatePosVel& est) const +void TrackerParticle::getEstimate(BFL::StatePosVel& est) const { - est = ((MCPdfPosVel*)(filter_->PostGet()))->ExpectedValueGet(); + est = static_cast(filter_->PostGet())->ExpectedValueGet(); }; - void TrackerParticle::getEstimate(people_msgs::PositionMeasurement& est) const { - StatePosVel tmp = filter_->PostGet()->ExpectedValueGet(); + BFL::StatePosVel tmp = filter_->PostGet()->ExpectedValueGet(); est.pos.x = tmp.pos_[0]; est.pos.y = tmp.pos_[1]; @@ -152,23 +135,17 @@ void TrackerParticle::getEstimate(people_msgs::PositionMeasurement& est) const est.object_id = getName(); } - - - - /// Get histogram from certain area Matrix TrackerParticle::getHistogramPos(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const { - return ((MCPdfPosVel*)(filter_->PostGet()))->getHistogramPos(min, max, step); + return static_cast(filter_->PostGet())->getHistogramPos(min, max, step); }; - Matrix TrackerParticle::getHistogramVel(const tf::Vector3& min, const tf::Vector3& max, const tf::Vector3& step) const { - return ((MCPdfPosVel*)(filter_->PostGet()))->getHistogramVel(min, max, step); + return static_cast(filter_->PostGet())->getHistogramVel(min, max, step); }; - double TrackerParticle::getLifetime() const { if (tracker_initialized_) @@ -177,7 +154,6 @@ double TrackerParticle::getLifetime() const return 0; } - double TrackerParticle::getTime() const { if (tracker_initialized_) @@ -185,7 +161,4 @@ double TrackerParticle::getTime() const else return 0; } -}; // namespace - - - +} // namespace estimation diff --git a/people_tracking_filter/src/uniform_vector.cpp b/people_tracking_filter/src/uniform_vector.cpp index bc3c28d3..80a1e67c 100644 --- a/people_tracking_filter/src/uniform_vector.cpp +++ b/people_tracking_filter/src/uniform_vector.cpp @@ -34,17 +34,16 @@ /* Author: Wim Meeussen */ -#include "people_tracking_filter/uniform_vector.h" +#include #include #include #include - -using namespace tf; +#include namespace BFL { -UniformVector::UniformVector(const Vector3& mu, const Vector3& size) - : Pdf (1), +UniformVector::UniformVector(const tf::Vector3& mu, const tf::Vector3& size) + : Pdf (1), mu_(mu), size_(size) { @@ -54,7 +53,6 @@ UniformVector::UniformVector(const Vector3& mu, const Vector3& size) probability_ = 1 / (size_[0] * 2 * size_[1] * 2 * size_[2] * 2); } - UniformVector::~UniformVector() {} UniformVector* UniformVector::Clone() const @@ -69,9 +67,7 @@ std::ostream& operator<< (std::ostream& os, const UniformVector& g) return os; } - - -Probability UniformVector::ProbabilityGet(const Vector3& input) const +Probability UniformVector::ProbabilityGet(const tf::Vector3& input) const { for (unsigned int i = 0; i < 3; i++) { @@ -81,30 +77,28 @@ Probability UniformVector::ProbabilityGet(const Vector3& input) const return probability_; } - bool -UniformVector::SampleFrom(vector >& list_samples, const int num_samples, int method, void * args) const +UniformVector::SampleFrom(vector >& list_samples, const int num_samples, + int method, void* args) const { list_samples.resize(num_samples); - vector >::iterator sample_it = list_samples.begin(); + vector >::iterator sample_it = list_samples.begin(); for (sample_it = list_samples.begin(); sample_it != list_samples.end(); sample_it++) SampleFrom(*sample_it, method, args); return true; } - bool -UniformVector::SampleFrom(Sample& one_sample, int method, void * args) const +UniformVector::SampleFrom(Sample& one_sample, int method, void * args) const { - one_sample.ValueSet(Vector3(((runif() - 0.5) * 2 * size_[0]) + mu_[0], - ((runif() - 0.5) * 2 * size_[1]) + mu_[1], - ((runif() - 0.5) * 2 * size_[2]) + mu_[2])); + one_sample.ValueSet(tf::Vector3(((runif() - 0.5) * 2 * size_[0]) + mu_[0], + ((runif() - 0.5) * 2 * size_[1]) + mu_[1], + ((runif() - 0.5) * 2 * size_[2]) + mu_[2])); return true; } - -Vector3 +tf::Vector3 UniformVector::ExpectedValueGet() const { return mu_; @@ -120,4 +114,4 @@ UniformVector::CovarianceGet() const return sigma; } -} // End namespace BFL +} // End namespace BFL diff --git a/people_velocity_tracker/CMakeLists.txt b/people_velocity_tracker/CMakeLists.txt index 2d7f427e..5ed5d66f 100644 --- a/people_velocity_tracker/CMakeLists.txt +++ b/people_velocity_tracker/CMakeLists.txt @@ -15,11 +15,17 @@ catkin_package( easy_markers geometry_msgs kalman_filter people_msgs roslib rospy ) -catkin_install_python(PROGRAMS scripts/static.py scripts/tracker.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +if(CATKIN_ENABLE_TESTING) + find_package(catkin REQUIRED COMPONENTS roslaunch roslint) + roslaunch_add_file_check(launch) + roslint_python() + roslint_add_test() +endif() + +install(FILES launch/filtered_tracked_detector.launch launch/tracked_detector.launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) -install(FILES - launch/filtered_tracked_detector.launch - launch/tracked_detector.launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +catkin_install_python(PROGRAMS scripts/static.py scripts/tracker.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/people_velocity_tracker/package.xml b/people_velocity_tracker/package.xml index 002a291d..e7c6bd0d 100644 --- a/people_velocity_tracker/package.xml +++ b/people_velocity_tracker/package.xml @@ -15,4 +15,6 @@ roslib rospy leg_detector + roslaunch + roslint diff --git a/people_velocity_tracker/scripts/static.py b/people_velocity_tracker/scripts/static.py index 769b809a..545e7c9d 100755 --- a/people_velocity_tracker/scripts/static.py +++ b/people_velocity_tracker/scripts/static.py @@ -1,34 +1,43 @@ #!/usr/bin/python -import roslib; roslib.load_manifest('people_velocity_tracker') +import argparse + +from people_msgs.msg import People, Person + import rospy -import sys -from people_msgs.msg import Person, People class VelocityTracker(object): - def __init__(self): + def __init__(self, x, y, vx, vy): self.ppub = rospy.Publisher('/people', People, queue_size=10) + self.person = Person() + self.person.position.x = x + self.person.position.y = y + self.person.position.z = 0.5 + self.person.velocity.x = vx + self.person.velocity.y = vy + self.person.name = 'static_test_person' + self.person.reliability = 0.90 def spin(self): rate = rospy.Rate(10) while not rospy.is_shutdown(): - pv = Person() pl = People() pl.header.stamp = rospy.Time.now() pl.header.frame_id = '/base_link' - pv.position.x = float(sys.argv[1]) - pv.position.y = float(sys.argv[2]) - pv.position.z = .5 - pv.velocity.x = float(sys.argv[3]) - pv.velocity.y = float(sys.argv[4]) - pv.name = 'asdf' - pv.reliability = .90 - pl.people.append(pv) - + pl.people.append(self.person) self.ppub.publish(pl) rate.sleep() -rospy.init_node("people_velocity_tracker") -vt = VelocityTracker() -vt.spin() + +if __name__ == '__main__': + rospy.init_node('people_velocity_tracker') + parser = argparse.ArgumentParser() + parser.add_argument('x', type=float, help='x coordinate') + parser.add_argument('y', type=float, help='y coordinate') + parser.add_argument('vx', type=float, nargs='?', default=0.0, help='x velocity') + parser.add_argument('vy', type=float, nargs='?', default=0.0, help='y velocity') + args = parser.parse_args() + + vt = VelocityTracker(args.x, args.y, args.vx, args.vy) + vt.spin() diff --git a/people_velocity_tracker/scripts/tracker.py b/people_velocity_tracker/scripts/tracker.py index 785c1724..da8c17dd 100755 --- a/people_velocity_tracker/scripts/tracker.py +++ b/people_velocity_tracker/scripts/tracker.py @@ -1,36 +1,47 @@ #!/usr/bin/python -import roslib; roslib.load_manifest('people_velocity_tracker') -import rospy -from geometry_msgs.msg import Point, Vector3 import math -from people_msgs.msg import PositionMeasurementArray, Person, People -from easy_markers.generator import MarkerGenerator, Marker + +from easy_markers.generator import Marker, MarkerGenerator + +from geometry_msgs.msg import Point, Vector3 + from kalman_filter import Kalman +from people_msgs.msg import People, Person, PositionMeasurementArray + +import rospy + + def distance(leg1, leg2): return math.sqrt(math.pow(leg1.x - leg2.x, 2) + math.pow(leg1.y - leg2.y, 2) + math.pow(leg1.z - leg2.z, 2)) + def average(leg1, leg2): return Point((leg1.x + leg2.x) / 2, (leg1.y + leg2.y) / 2, (leg1.z + leg2.z) / 2) + def add(v1, v2): return Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z) + def subtract(v1, v2): return Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z) + def scale(v, s): v.x *= s v.y *= s v.z *= s + def printv(v): - print "%.2f %.2f %.2f"%(v.x, v.y, v.z), + print('%.2f %.2f %.2f' % (v.x, v.y, v.z),) + gen = MarkerGenerator() gen.type = Marker.ARROW @@ -58,12 +69,12 @@ def update(self, msg): def age(self): return self.pos.header.stamp - def id(self): + def get_id(self): return self.pos.object_id def velocity(self): k = self.k.values() - if k == None: + if k is None: return Vector3() v = Vector3(k[0], k[1], k[2]) return v @@ -72,14 +83,13 @@ def publish_markers(self, pub): gen.scale = [.1, .3, 0] gen.color = [1, 1, 1, 1] vel = self.velocity() - #scale(vel, 15) m = gen.marker(points=[self.pos.pos, add(self.pos.pos, vel)]) m.header = self.pos.header pub.publish(m) def get_person(self): p = Person() - p.name = self.id() + p.name = self.get_id() p.position = self.pos.pos p.velocity = self.velocity() p.reliability = self.reliability @@ -132,6 +142,8 @@ def publish(self): self.ppub.publish(pl) -rospy.init_node("people_velocity_tracker") -vt = VelocityTracker() -vt.spin() + +if __name__ == '__main__': + rospy.init_node('people_velocity_tracker') + vt = VelocityTracker() + vt.spin()