Detectcone cleanup

CMakeLists.txt

@@ -57,18 +57,18 @@ find_package(Threads REQUIRED)
 # Extract cluon-msc from cluon-complete.hpp.
 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 ${CMAKE_CURRENT_SOURCE_DIR}/include/${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 ${CMAKE_CURRENT_SOURCE_DIR}/include/${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
     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-sources --cpp-add-include-file=opendlv-standard-message-set.hpp --out=${CMAKE_BINARY_DIR}/opendlv-standard-message-set.cpp ${CMAKE_CURRENT_SOURCE_DIR}/include/${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}/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})
@@ -79,15 +79,13 @@ include_directories(SYSTEM ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty)
 
 ################################################################################
 # 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 +122,13 @@ 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