visualize reconstruction

CMakeLists.txt

@@ -15,6 +15,7 @@ set(REQ_CATKIN_PKGS
   cv_bridge
   sensor_msgs
   image_transport
+  pcl_ros
   tf2_ros
   rosbag
 )
@@ -28,6 +29,11 @@ find_package(OpenCV 3.3 REQUIRED)
 include_directories(${OpenCV_INCLUDE_DIRS})
 link_directories(${OpenCV_LIBRARIES_DIRS})
 
+find_package(PCL 1.7 REQUIRED COMPONENTS common)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
 find_package(HDF5 COMPONENTS C CXX HL REQUIRED)
 include_directories(${HDF5_INCLUDE_DIR})
 link_directories(${HDF5_LIBRARY_DIRS})
@@ -88,20 +94,23 @@ target_link_libraries(omni_slam_tracking_eval_node PUBLIC omni_slam_eval_lib
   ${OpenCV_LIBS}
   ${HDF5_CXX_LIBRARIES}
   ${OpenMP_CXX_FLAGS}
+  ${PCL_LIBRARIES}
 )
 
 target_link_libraries(omni_slam_matching_eval_node PUBLIC omni_slam_eval_lib
   ${catkin_LIBRARIES}
   ${OpenCV_LIBS}
   ${HDF5_CXX_LIBRARIES}
   ${OpenMP_CXX_FLAGS}
+  ${PCL_LIBRARIES}
 )
 
 target_link_libraries(omni_slam_reconstruction_eval_node PUBLIC omni_slam_eval_lib
   ${catkin_LIBRARIES}
   ${OpenCV_LIBS}
   ${HDF5_CXX_LIBRARIES}
   ${OpenMP_CXX_FLAGS}
+  ${PCL_LIBRARIES}
 )
 
 target_compile_options(omni_slam_tracking_eval_node PUBLIC ${OpenMP_CXX_FLAGS})

launch/reconstruction_eval.launch

@@ -16,14 +16,14 @@
             camera_model: '$(arg camera_model)'
             camera_parameters: $(arg camera_params)
             detector_type: 'GFTT'
-            detector_parameters: {maxCorners: 100, qualityLevel: 0.01, minDistance: 5, blockSize: 5}
+            detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             tracker_window_size: 128
             tracker_num_scales: 4
             tracker_delta_pixel_error_threshold: 3.0
             tracker_error_threshold: 20.0
             min_features_per_region: 10
             max_reprojection_error: 0.0
-            min_triangulation_angle: 5.0
+            min_triangulation_angle: 3.0
         </rosparam>
     </node>
 </launch>

package.xml

@@ -14,5 +14,6 @@
   <depend>sensor_msgs</depend>
   <depend>image_transport</depend>
   <depend>tf2_ros</depend>
+  <depend>pcl_ros</depend>
   <depend>rosbag</depend>
 </package>

src/data/landmark.cc

@@ -75,6 +75,7 @@ const Feature* Landmark::GetObservationByFrameID(const int frame_id) const
 void Landmark::SetEstimatedPosition(Vector3d pos)
 {
     posEstimate_ = pos;
+    hasPosEstimate_ = true;
 }
 
 Vector3d Landmark::GetGroundTruth() const
@@ -94,7 +95,7 @@ bool Landmark::HasGroundTruth() const
 
 bool Landmark::HasEstimatedPosition() const
 {
-    return hasGroundTruth_;
+    return hasPosEstimate_;
 }
 
 }

src/module/reconstruction_module.cc

@@ -20,15 +20,80 @@ ReconstructionModule::ReconstructionModule(std::unique_ptr<reconstruction::Trian
 void ReconstructionModule::Update(std::vector<data::Landmark> &landmarks)
 {
     triangulator_->Triangulate(landmarks);
+
+    visualization_.Reserve(landmarks.size());
+    for (int i = 0; i < lastLandmarksSize_; i++)
+    {
+        if (landmarks[i].HasEstimatedPosition())
+        {
+            visualization_.UpdatePoint(i, landmarks[i].GetEstimatedPosition());
+        }
+    }
+    for (int i = lastLandmarksSize_; i < landmarks.size(); i++)
+    {
+        cv::Point2f pt = landmarks[i].GetObservations()[0].GetKeypoint().pt;
+        visualization_.AddPoint(pt);
+    }
+    lastLandmarksSize_ = landmarks.size();
 }
 
 ReconstructionModule::Stats& ReconstructionModule::GetStats()
 {
     return stats_;
 }
 
-void ReconstructionModule::Visualize()
+void ReconstructionModule::Visualize(cv::Mat &img, pcl::PointCloud<pcl::PointXYZRGB> &cloud)
+{
+    visualization_.OutputPointCloud(img, cloud);
+}
+
+void ReconstructionModule::Visualization::UpdatePoint(int index, const Vector3d &point)
+{
+    cloud_.at(index).x = point(0);
+    cloud_.at(index).y = point(1);
+    cloud_.at(index).z = point(2);
+    goodIndices_.insert(index);
+}
+
+void ReconstructionModule::Visualization::AddPoint(const cv::Point2f &pix)
+{
+    pcl::PointXYZRGB pt(0, 0, 0);
+    cloud_.push_back(pt);
+    newPts_.push_back(pix);
+    numNewPts_++;
+}
+
+void ReconstructionModule::Visualization::Reserve(int size)
+{
+    cloud_.reserve(size);
+}
+
+void ReconstructionModule::Visualization::OutputPointCloud(cv::Mat &img, pcl::PointCloud<pcl::PointXYZRGB> &cloud)
 {
+    for (int i = cloud_.size() - numNewPts_; i < cloud_.size(); i++)
+    {
+        cv::Point2f &pt = newPts_[i - cloud_.size() + numNewPts_];
+        if (img.channels() == 3)
+        {
+            cloud_.at(i).r = img.at<cv::Vec3b>(pt.y, pt.x)[2];
+            cloud_.at(i).g = img.at<cv::Vec3b>(pt.y, pt.x)[1];
+            cloud_.at(i).b = img.at<cv::Vec3b>(pt.y, pt.x)[0];
+        }
+        else
+        {
+            cloud_.at(i).r = img.at<unsigned char>(pt.y, pt.x);
+            cloud_.at(i).g = img.at<unsigned char>(pt.y, pt.x);
+            cloud_.at(i).b = img.at<unsigned char>(pt.y, pt.x);
+        }
+    }
+    numNewPts_ = 0;
+    newPts_.clear();
+    cloud.clear();
+    cloud.reserve(goodIndices_.size());
+    for (int inx : goodIndices_)
+    {
+        cloud.push_back(cloud_.at(inx));
+    }
 }
 
 }

src/module/reconstruction_module.h

@@ -2,11 +2,17 @@
 #define _RECONSTRUCTION_MODULE_H_
 
 #include <vector>
+#include <set>
 #include <memory>
 
 #include "reconstruction/triangulator.h"
 #include "data/landmark.h"
 
+#include <pcl/common/projection_matrix.h>
+#include <pcl/point_types.h>
+
+using namespace Eigen;
+
 namespace omni_slam
 {
 namespace module
@@ -25,17 +31,30 @@ class ReconstructionModule
     void Update(std::vector<data::Landmark> &landmarks);
 
     Stats& GetStats();
-    void Visualize();
+    void Visualize(cv::Mat &img, pcl::PointCloud<pcl::PointXYZRGB> &cloud);
 
 private:
     class Visualization
     {
+    public:
+        void UpdatePoint(int index, const Vector3d &point);
+        void AddPoint(const cv::Point2f &pix);
+        void Reserve(int size);
+        void OutputPointCloud(cv::Mat &img, pcl::PointCloud<pcl::PointXYZRGB> &cloud);
+
+    private:
+        pcl::PointCloud<pcl::PointXYZRGB> cloud_;
+        std::vector<cv::Point2f> newPts_;
+        int numNewPts_{0};
+        std::set<int> goodIndices_;
     };
 
     std::shared_ptr<reconstruction::Triangulator> triangulator_;
 
     Stats stats_;
     Visualization visualization_;
+
+    int lastLandmarksSize_{0};
 };
 
 }

src/ros/reconstruction_eval.cc

@@ -3,6 +3,9 @@
 #include "reconstruction/triangulator.h"
 #include "module/tracking_module.h"
 
+#include <pcl_conversions/pcl_conversions.h>
+#include "sensor_msgs/PointCloud2.h"
+
 using namespace std;
 
 namespace omni_slam
@@ -27,6 +30,9 @@ ReconstructionEval::ReconstructionEval(const ::ros::NodeHandle &nh, const ::ros:
 void ReconstructionEval::InitPublishers()
 {
     TrackingEval::InitPublishers();
+    string outputTopic;
+    nhp_.param("point_cloud_topic", outputTopic, string("/omni_slam/reconstructed"));
+    pointCloudPublisher_ = nh_.advertise<sensor_msgs::PointCloud2>(outputTopic, 2);
 }
 
 void ReconstructionEval::ProcessFrame(unique_ptr<data::Frame> &&frame)
@@ -42,6 +48,13 @@ void ReconstructionEval::GetResultsData(std::map<std::string, std::vector<std::v
 
 void ReconstructionEval::Visualize(cv_bridge::CvImagePtr &base_img)
 {
+    pcl::PointCloud<pcl::PointXYZRGB> cloud;
+    reconstructionModule_->Visualize(base_img->image, cloud);
+    sensor_msgs::PointCloud2 msg;
+    pcl::toROSMsg(cloud, msg);
+    msg.header.frame_id = "map";
+    msg.header.stamp = ::ros::Time::now();
+    pointCloudPublisher_.publish(msg);
     TrackingEval::Visualize(base_img);
 }
 

src/ros/reconstruction_eval.h

@@ -25,6 +25,8 @@ class ReconstructionEval : public TrackingEval
     void GetResultsData(std::map<std::string, std::vector<std::vector<double>>> &data);
     void Visualize(cv_bridge::CvImagePtr &base_img);
 
+    ::ros::Publisher pointCloudPublisher_;
+
     std::unique_ptr<module::ReconstructionModule> reconstructionModule_;
 };