Detectcone cleanup

CMakeLists.txt

@@ -20,8 +20,8 @@ project(cfsd18-perception-detectcone)
 ################################################################################
 # Defining the relevant versions of OpenDLV Standard Message Set and libcluon.
 set(OPENDLV_STANDARD_MESSAGE_SET opendlv-standard-message-set-v0.9.1.odvd)
-set(CLUON_COMPLETE cluon-complete-v0.0.102.hpp)
-
+set(CLUON_COMPLETE cluon-complete-v0.0.114.hpp)
+set(CLUON_PATH ${CMAKE_CURRENT_SOURCE_DIR})
 ################################################################################
 # This project requires C++14 or newer.
 set(CMAKE_CXX_STANDARD 14)
@@ -55,39 +55,41 @@ set(THREADS_PREFER_PTHREAD_FLAG ON)
 find_package(Threads REQUIRED)
 ################################################################################
 # Extract cluon-msc from cluon-complete.hpp.
+
+execute_process(COMMAND wget -P ${CLUON_PATH} https://raw.githubusercontent.com/chrberger/libcluon/gh-pages/headeronly/${CLUON_COMPLETE})
+
 add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/cluon-msc
     WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
-    COMMAND ${CMAKE_COMMAND} -E create_symlink ${CMAKE_CURRENT_SOURCE_DIR}/src/${CLUON_COMPLETE} ${CMAKE_BINARY_DIR}/cluon-complete.hpp
+    COMMAND ${CMAKE_COMMAND} -E create_symlink ${CLUON_PATH}/${CLUON_COMPLETE} ${CMAKE_BINARY_DIR}/cluon-complete.hpp
     COMMAND ${CMAKE_COMMAND} -E create_symlink ${CMAKE_BINARY_DIR}/cluon-complete.hpp ${CMAKE_BINARY_DIR}/cluon-complete.cpp
     COMMAND ${CMAKE_CXX_COMPILER} -o ${CMAKE_BINARY_DIR}/cluon-msc ${CMAKE_BINARY_DIR}/cluon-complete.cpp -std=c++14 -pthread -D HAVE_CLUON_MSC
-    DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/src/${CLUON_COMPLETE})
+    DEPENDS ${CLUON_PATH}/${CLUON_COMPLETE})
 
 ################################################################################
 # Generate opendlv-standard-message-set.{hpp,cpp} from ${OPENDLV_STANDARD_MESSAGE_SET} file.
-add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/opendlv-standard-message-set.cpp
+add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/opendlv-standard-message-set.hpp
     WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
-    COMMAND ${CMAKE_BINARY_DIR}/cluon-msc --cpp-sources --cpp-add-include-file=opendlv-standard-message-set.hpp --out=${CMAKE_BINARY_DIR}/opendlv-standard-message-set.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/${OPENDLV_STANDARD_MESSAGE_SET}
-    COMMAND ${CMAKE_BINARY_DIR}/cluon-msc --cpp-headers --out=${CMAKE_BINARY_DIR}/opendlv-standard-message-set.hpp ${CMAKE_CURRENT_SOURCE_DIR}/src/${OPENDLV_STANDARD_MESSAGE_SET}
-    DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/src/${OPENDLV_STANDARD_MESSAGE_SET} ${CMAKE_BINARY_DIR}/cluon-msc)
+    COMMAND ${CMAKE_BINARY_DIR}/cluon-msc --cpp --out=${CMAKE_BINARY_DIR}/opendlv-standard-message-set.hpp ${CMAKE_CURRENT_SOURCE_DIR}/include/${OPENDLV_STANDARD_MESSAGE_SET}
+    DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/include/${OPENDLV_STANDARD_MESSAGE_SET} ${CMAKE_BINARY_DIR}/cluon-msc)
 
 # Add current build directory as include directory as it contains generated files.
 include_directories(SYSTEM ${CMAKE_BINARY_DIR})
 include_directories(SYSTEM ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty)
 
 ################################################################################
 # Gather all object code first to avoid double compilation. This needs to be done for all added objects, can become very long? Why is this object and not static which works
-
+add_library(${PROJECT_NAME}-core STATIC ${CMAKE_CURRENT_SOURCE_DIR}/src/detectcone.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/collector.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/cone.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/point.cpp)
+add_custom_target(generate_opendlv_standard_message_set_hpp DEPENDS ${CMAKE_BINARY_DIR}/opendlv-standard-message-set.hpp)
+add_dependencies(${PROJECT_NAME}-core generate_opendlv_standard_message_set_hpp)
 ################################################################################
 # OpenCV
-#set(CMAKE_PREFIX_PATH "/usr/local/opt/opencv3/share/OpenCV")
 find_package(OpenCV REQUIRED)
 include_directories(${OpenCV_INCLUDE_DIRS})
 
-
 ################################################################################
 # Eigen
-find_package(Eigen3 REQUIRED NO_MODULE)
-include_directories(${Eigen3_INCLUDE_DIRS})
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
+include_directories(SYSTEM "/usr/local/include/eigen3")
 
 ###############################################################################
 # OpenMP
@@ -124,14 +126,12 @@ include_directories(SYSTEM /usr/local/include)
 link_directories(SYSTEM /usr/lib) #Access g2o
 link_directories(SYSTEM /usr/local/lib) #Access g2o
 
-include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src)
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
 
-add_library(${PROJECT_NAME}-core STATIC ${CMAKE_CURRENT_SOURCE_DIR}/src/detectcone.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/collector.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/cone.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/point.cpp ${CMAKE_BINARY_DIR}/opendlv-standard-message-set.cpp)
 
 # Add custom libs h
-set(LIBRARIES ${LIBRARIES} Threads::Threads ${OpenCV_LIBS} ${Eigen3_LIBS} ${TinyDNN_LIBS} ${TBB_LIBRARIES} rt) 
+set(LIBRARIES ${LIBRARIES} Threads::Threads ${OpenCV_LIBS} ${Eigen3_LIBS} ${TinyDNN_LIBS} ${TBB_LIBRARIES} rt)
 
-MESSAGE(STATUS "heej")
 MESSAGE(STATUS ${LIBRARIES})
 #${TBB_LIBRARIES}
 

Dockerfile.amd64

@@ -13,7 +13,7 @@
 # You should have received a copy of the GNU General Public License
 # along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-# Part to build opendlv-perception-vision-orbslam2. 
+# Part to build opendlv-perception-vision-orbslam2.
 FROM alpine:edge as builder
 MAINTAINER Christian Berger "[email protected]"
 #Get OS stuff
@@ -36,7 +36,7 @@ WORKDIR /tmp
 ADD ./config.h /opt/sources/config.h
 
 RUN git clone https://github.com/justusc/FindTBB.git && \
-   cp FindTBB/FindTBB.cmake /usr/share/cmake/Modules/ 
+   cp FindTBB/FindTBB.cmake /usr/share/cmake/Modules/
 
 RUN git clone https://github.com/tiny-dnn/tiny-dnn.git && \
   mkdir -p tiny-dnn/build && \

include/detectcone.hpp

@@ -17,127 +17,125 @@
  * USA.
  */
 
-#ifndef OPENDLV_LOGIC_CFSD18_SENSATION_ATTENTION_HPP
-#define OPENDLV_LOGIC_CFSD18_SENSATION_ATTENTION_HPP
-
-#include <opendavinci/odcore/base/module/DataTriggeredConferenceClientModule.h>
-#include <opendavinci/odcore/data/Container.h>
-#include <opendavinci/odcore/base/Mutex.h>
-
-#include <odvdopendlvstandardmessageset/GeneratedHeaders_ODVDOpenDLVStandardMessageSet.h>
-#include <odvdcfsd18/GeneratedHeaders_ODVDcfsd18.h>
-
-#include "opendavinci/generated/odcore/data/SharedPointCloud.h"
-#include "opendavinci/generated/odcore/data/CompactPointCloud.h"
-
-#include <opendavinci/odcore/strings/StringToolbox.h>
-#include <opendavinci/odcore/wrapper/Eigen.h>
-#include <opendlv/data/environment/Point3.h> 
-
-namespace opendlv {
-namespace logic {
-namespace cfsd18 {
-namespace sensation {
-
-
-using namespace std;
-using namespace odcore::base;
-using namespace odcore::data;
-using namespace odcore::wrapper;
-
-
-class Attention : public odcore::base::module::DataTriggeredConferenceClientModule {
+#ifndef CFSD18_PERCEPTION_DETECTCONE_HPP
+#define CFSD18_PERCEPTION_DETECTCONE_HPP
+
+#include <iostream>
+#include <memory>
+#include <vector>
+#include <cmath>
+#include <thread>
+#include <Eigen/Dense>
+#include <cstdint>
+#include <tuple>
+#include <utility>
+#include <string>
+#include <sstream>
+#include <dirent.h>
+#include <ctime>
+
+#include "cluon-complete.hpp"
+#include "opendlv-standard-message-set.hpp"
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <tiny_dnn/tiny_dnn.h>
+
+#include "cone.hpp"
+#include "point.hpp"
+
+typedef std::tuple<opendlv::logic::perception::ObjectDirection,opendlv::logic::perception::ObjectDistance,opendlv::logic::perception::ObjectType> ConePackage;
+
+class DetectCone {
  public:
-  Attention(int32_t const &, char **);
-  Attention(Attention const &) = delete;
-  Attention &operator=(Attention const &) = delete;
-  virtual ~Attention();
-  virtual void nextContainer(odcore::data::Container &);
+  DetectCone(std::map<std::string, std::string> commandlineArguments, cluon::OD4Session &od4);
+  DetectCone(DetectCone const &) = delete;
+  DetectCone &operator=(DetectCone const &) = delete;
+  ~DetectCone();
+  void nextContainer(cluon::data::Envelope data);
+  void receiveCombinedMessage(cluon::data::TimeStamp currentFrameTime,std::map<int,ConePackage> currentFrame);
+  void checkLidarState();
+  bool getdrivingState();
+  void setTimeStamp(std::pair<int64_t, cv::Mat>);
+  void getTimeStamp(const std::string);
+  void setStateMachineStatus(cluon::data::Envelope data); 
+  void getFolderName(const std::string folderName);
+  void saveImages(std::string saveString, cv::Mat img);
 
  private:
-  void setUp();
-  void tearDown();
+  void setUp(std::map<std::string, std::string> commandlineArguments);
+  void blockMatching(cv::Mat&, cv::Mat, cv::Mat);
+  void reconstruction(cv::Mat, cv::Mat&, cv::Mat&, cv::Mat&);
+  void convertImage(cv::Mat, int, int, tiny_dnn::vec_t&);
+  void CNN(const std::string&, tiny_dnn::network<tiny_dnn::sequential>&);
+  void imRegionalMax(std::vector<Cone>&, size_t, cv::Mat, int, double, int);
+  cv::Point3f median(std::vector<cv::Point3f> vec3);
+  void gather_points(cv::Mat, std::vector<float>, std::vector<int>&, std::vector<float>&);
+  void filterKeypoints(std::vector<cv::Point3f>&);
+  int xyz2xy(cv::Mat Q, cv::Point3f xyz, cv::Point& xy, float radius);
+  int countFiles(const char*);
+  void annotate(cv::Mat, int, cv::Point, int);
+  void forwardDetectionORB(cv::Mat img);
+  void backwardDetection(cv::Mat, Eigen::MatrixXd&, int64_t);
+  std::vector<Cone> MatchCones(std::vector<Cone>);
+
+  Eigen::MatrixXd Spherical2Cartesian(double, double, double);
+  void Cartesian2Spherical(double, double, double, opendlv::logic::sensation::Point&);
+  void LidarToCoG(opendlv::logic::sensation::Point& conePoint);
+  void CameraToCoG(opendlv::logic::sensation::Point& conePoint);
+  void SendCollectedCones(Eigen::MatrixXd);
+  void SendMatchedContainer(std::vector<Cone>);
+
+  cluon::OD4Session &m_od4;
+  float m_threshold;
+  cluon::data::TimeStamp m_coneTimeStamp;
+  int64_t m_imgTimeStamp;
+  cluon::data::TimeStamp m_start = {};
+  Eigen::MatrixXd m_coneCollector;
+  uint32_t m_lastObjectId;
+  std::mutex m_coneMutex;
+  std::mutex m_imgMutex;
+  std::mutex m_stateMachineMutex;
+  bool m_recievedFirstImg;
+  cv::Mat m_img;
+  std::vector<std::pair<int64_t, cv::Mat>> m_imgAndTimeStamps;
+  std::vector<std::pair<bool,Cone>> m_coneFrame = {};
+  std::vector<int64_t> m_timeStamps;
+  int m_count;
+  int m_count2;
+  uint32_t m_currentFrame;
+  bool m_offline;
+  bool m_annotate;
+  bool m_verbose;
+  bool m_forwardDetection;
+  bool m_drivingState;
+  bool m_readyStateMachine;
+  tiny_dnn::network<tiny_dnn::sequential> m_model;
+  bool m_lidarIsWorking;
+  int64_t m_checkLidarMilliseconds;
+  uint32_t m_senderStamp = 118;
+  uint32_t m_attentionSenderStamp = 116;
+  int m_patchSize;
+  int m_width;
+  int m_height;
+  double m_xShift;                //lateral distance between camera and LiDAR 
+  double m_yShift;            //Height difference between camera and LiDAR
+  double m_zShift;        //Distance between camera and LiDAR in forward distance
+  int m_fastThreshold;
+  float m_matchDistance;
+  int m_orbPatchSize;
+  std::string m_folderName;
+  float m_maxZ;
+  std::ofstream m_file;
 
-  void SaveOneCPCPointNoIntensity(const int &pointIndex,const uint16_t &distance_integer, const double &azimuth, const double &verticalAngle, const uint8_t &distanceEncoding);
-  //void SaveCPC32NoIntensity(const uint8_t &part, const uint8_t &entriesPerAzimuth, const double &startAzimuth, const double &endAzimuth, const uint8_t &distanceEncoding);
-  void SavePointCloud();
-  void ConeDetection();
-  vector<vector<uint32_t>> NNSegmentation(MatrixXd &pointCloudConeROI, const double &connectDistanceThreshold);
-  vector<vector<uint32_t>> FindConesFromObjects(MatrixXd &pointCloudConeROI, vector<vector<uint32_t>> &objectIndexList, const double &minNumOfPointsForCone, const double &maxNumOfPointsForCone, const double &nearConeRadiusThreshold, const double &farConeRadiusThreshold, const double &zRangeThreshold);
-  MatrixXd ExtractConeROI(const double &xBoundary, const double &yBoundary, const double &groundLayerZ,  const double &coneHeight);
-  double CalculateXYDistance(Eigen::MatrixXd &pointCloud, const uint32_t &index1, const uint32_t &index2);
-  double CalculateConeRadius(Eigen::MatrixXd &potentialConePointCloud);
-  double GetZRange(Eigen::MatrixXd &potentialConePointCloud);
-  void SendingConesPositions(Eigen::MatrixXd &pointCloudConeROI, vector<vector<uint32_t>> &coneIndexList);
-  opendlv::logic::sensation::Point Cartesian2Spherical(double &x, double &y, double &z);
-  Eigen::MatrixXd RANSACRemoveGround(MatrixXd);
-  Eigen::MatrixXd RemoveDuplicates(MatrixXd);
-
-
-  // Define constants to decode CPC message
-  const double START_V_ANGLE = -15.0; //For each azimuth there are 16 points with unique vertical angles from -15 to 15 degrees
-  const double V_INCREMENT = 2.0; //The vertical angle increment for the 16 points with the same azimuth is 2 degrees
-  const double START_V_ANGLE_32 = -30.67; //The starting angle for HDL-32E. Vertical angle ranges from -30.67 to 10.67 degress, with alternating increment 1.33 and 1.34
-  const double V_INCREMENT_32_A = 1.33; //The first vertical angle increment for HDL-32E
-  const double V_INCREMENT_32_B = 1.34; //The second vertical angle increment for HDL-32E
-
-  // Constants for degree transformation
   const double DEG2RAD = 0.017453292522222; // PI/180.0
   const double RAD2DEG = 57.295779513082325; // 1.0 / DEG2RAD;
-
-  // Variables to handle HDL32 3 parts of message
-  uint8_t m_12_startingSensorID_32; //From which layer for the first part(12 layers) of CPC for HDL-32E
-  uint8_t m_11_startingSensorID_32; //From which layer for the second part(11 layers) of CPC for HDL-32E
-  uint8_t m_9_startingSensorID_32; //From which layer for the third part(9 layers) of CPC for HDL-32E
-  array<double, 12>  m_12_verticalAngles; //Store the 12 vertical angles for the first part (including 12 layers) of CPC for HDL-32E
-  array<double, 11> m_11_verticalAngles; //Store the 11 vertical angles for the second part (including 11 layers) of CPC for HDL-32E
-  array<double, 9> m_9_verticalAngles; //Store the 9 vertical angles for the third part (including 9 layers) of CPC for HDL-32E
-  string m_12_cpcDistance_32; //The distance string for the first part of CPC for HDL-32E    
-  string m_11_cpcDistance_32; //The distance string for the second part of CPC for HDL-32E    
-  string m_9_cpcDistance_32; //The distance string for the third part of CPC for HDL-32E
-  uint64_t m_previousCPC32TimeStamp;//The sample time of the previous CPC container belonging to a HDL-32E scan
-  uint8_t m_cpcMask_32; //The lowest 3 bits represent which part(s) of HDL-32E CPC of the same scan has been received. 0100 means the first part has arrived; 0010 means the second part has arrived; 0001 means the third part has arrived.
-
-  // Class variables to store Lidar message
-  CompactPointCloud m_cpc;
-  odcore::base::Mutex m_cpcMutex;
-  bool m_SPCReceived;//Set to true when the first shared point cloud is received
-  bool m_CPCReceived;//Set to true when the first compact point cloud is received
-  uint32_t m_recordingYear;//The year when a recording with CPC was taken
-
-  // Class variables to save point cloud 
-  MatrixXd m_pointCloud;
-  int m_pointIndex;
-  // Define constants and thresolds forclustering algorithm
-  double m_xBoundary;
-  double m_yBoundary;
-  double m_groundLayerZ;
-  double m_coneHeight;
-  double m_connectDistanceThreshold;
-  double m_layerRangeThreshold;
-  uint16_t m_minNumOfPointsForCone;
-  uint16_t m_maxNumOfPointsForCone;
-  double m_farConeRadiusThreshold;
-  double m_nearConeRadiusThreshold;
-  double m_zRangeThreshold;
-  odcore::data::TimeStamp m_CPCReceivedLastTime;
-  double m_algorithmTime;
-  Eigen::MatrixXd m_generatedTestPointCloud;
-  // RANSAC thresholds
-  double m_inlierRangeThreshold;
-  double m_inlierFoundTreshold;
-  double m_ransacIterations;
-  double m_dotThreshold;
-  uint32_t m_senderStamp = 0;
-  Eigen::MatrixXd m_lastBestPlane;
-
-
-
+  const double PI = 3.14159265;
 };
 
-}
-}
-}
-}
 
-#endif
+#endif