found bug in math. Fixed it for SphereCorrectors. TODO: fix for all

CMakeLists.txt

@@ -29,6 +29,7 @@ find_package(catkin REQUIRED COMPONENTS
     tf2_ros
     rmcl_msgs
     image_transport
+    visualization_msgs
 )
 
 include_directories(

include/rmcl/correction/MICP.hpp

@@ -157,6 +157,22 @@ class MICP
         return m_sensors;
     }
 
+    void useInThisThread()
+    {
+        #ifdef RMCL_OPTIX
+        // TODO hold a context ptr at MICP object
+        if(m_map_optix)
+        {
+            auto cuda_ctx = m_map_optix->scene()->stream()->context();
+            if(!cuda_ctx->isActive())
+            {
+                std::cout << "MICP - reactivating CUDA for thread" << std::endl;
+                cuda_ctx->use();
+            }
+        }
+        #endif // RMCL_OPTIX
+    }
+
 protected:
     bool checkTF(bool prints = false);
 

include/rmcl/correction/MICPRangeSensor.hpp

@@ -157,10 +157,13 @@ class MICPRangeSensor
     size_t      n_ranges_valid = 0;
 
 
+
     // subscriber to data
     NodeHandlePtr nh;
+    NodeHandlePtr nh_p;
     SubscriberPtr data_sub;
     SubscriberPtr info_sub;
+
 
     ImageTransportPtr it;
     ITSubscriberPtr img_sub;
@@ -173,6 +176,10 @@ class MICPRangeSensor
     float                       adaptive_max_dist_min = 0.15;
     float                       corr_weight = 1.0;
 
+    // DEBUGGING
+    bool            draw_correspondences = true;
+    PublisherPtr    pub_corr;
+
     // correction: TODO better
     #ifdef RMCL_EMBREE
     SphereCorrectorEmbreePtr     corr_sphere_embree;

include/rmcl/correction/SphereCorrectorEmbree.hpp

@@ -92,6 +92,20 @@ class SphereCorrectorEmbree
         const rmagine::MemoryView<rmagine::Transform, rmagine::RAM>& Tbms
     );
 
+    void findCorrespondences(
+        const rmagine::MemoryView<rmagine::Transform, rmagine::RAM>& Tbms,
+        rmagine::MemoryView<rmagine::Point> dataset_points,
+        rmagine::MemoryView<rmagine::Point> model_points,
+        rmagine::MemoryView<unsigned int> corr_valid
+    );
+
+    void findCorrespondences(
+        const rmagine::MemoryView<rmagine::Transform, rmagine::RAM>& Tbms,
+        rmagine::Memory<rmagine::Point>& dataset_points,
+        rmagine::Memory<rmagine::Point>& model_points,
+        rmagine::Memory<unsigned int>& corr_valid
+    );
+
     // TODO: camel case
     void compute_covs(
         const rmagine::MemoryView<rmagine::Transform, rmagine::RAM>& Tbms,
@@ -121,6 +135,17 @@ class SphereCorrectorEmbree
         const rmagine::MemoryView<rmagine::Transform, rmagine::RAM>& Tbms, 
         size_t Ntests = 100);
 
+    // TODO: add properly - rmagine
+    inline CorrectionParams params() const
+    {
+        return m_params;
+    }
+
+    inline rmagine::SphericalModel model() const
+    {
+        return m_model[0];
+    }
+
 protected:
     rmagine::Memory<float, rmagine::RAM> m_ranges;
 

include/rmcl/correction/SphereCorrectorOptix.hpp

@@ -72,10 +72,26 @@ class SphereCorrectorOptix
         const rmagine::MemoryView<float, rmagine::VRAM_CUDA>& ranges
     );
 
+
+
     CorrectionResults<rmagine::VRAM_CUDA> correct(
         const rmagine::MemoryView<rmagine::Transform, rmagine::VRAM_CUDA>& Tbms
     ) const;
 
+    void findCorrespondences(
+        const rmagine::MemoryView<rmagine::Transform, rmagine::VRAM_CUDA>& Tbms,
+        rmagine::MemoryView<rmagine::Point, rmagine::VRAM_CUDA> dataset_points,
+        rmagine::MemoryView<rmagine::Point, rmagine::VRAM_CUDA> model_points,
+        rmagine::MemoryView<unsigned int, rmagine::VRAM_CUDA> corr_valid
+    ) const;
+
+    void findCorrespondences(
+        const rmagine::MemoryView<rmagine::Transform, rmagine::VRAM_CUDA>& Tbms,
+        rmagine::Memory<rmagine::Point, rmagine::VRAM_CUDA>& dataset_points,
+        rmagine::Memory<rmagine::Point, rmagine::VRAM_CUDA>& model_points,
+        rmagine::Memory<unsigned int, rmagine::VRAM_CUDA>& corr_valid
+    ) const;
+
     void compute_covs(
         const rmagine::MemoryView<rmagine::Transform, rmagine::VRAM_CUDA>& Tbms,
         rmagine::MemoryView<rmagine::Vector, rmagine::VRAM_CUDA>& ms,
@@ -92,8 +108,6 @@ class SphereCorrectorOptix
     CorrectionPreResults<rmagine::VRAM_CUDA> compute_covs(
         const rmagine::MemoryView<rmagine::Transform, rmagine::VRAM_CUDA>& Tbms
     ) const;
-
-
     struct Timings
     {   
         double sim = 0.0;
@@ -105,6 +119,20 @@ class SphereCorrectorOptix
         const rmagine::MemoryView<rmagine::Transform, rmagine::VRAM_CUDA>& Tbms, 
         size_t Ntests = 100);
 
+
+    // TODO: add properly - rmagine
+    inline CorrectionParams params() const
+    {
+        rmagine::Memory<CorrectionParams, rmagine::RAM> params_ = m_params;
+        return params_[0];
+    }
+
+    inline rmagine::SphericalModel model() const
+    {
+        rmagine::Memory<rmagine::SphericalModel, rmagine::RAM> model_ = m_model;
+        return model_[0];
+    }
+
 protected:
     rmagine::Memory<float, rmagine::VRAM_CUDA> m_ranges;
     rmagine::Memory<CorrectionParams, rmagine::VRAM_CUDA> m_params;

include/rmcl/util/ros_defines.h

@@ -51,6 +51,7 @@ namespace rmcl
 
 using NodeHandlePtr = std::shared_ptr<ros::NodeHandle>;
 using SubscriberPtr = std::shared_ptr<ros::Subscriber>;
+using PublisherPtr = std::shared_ptr<ros::Publisher>;
 using ImageTransportPtr = std::shared_ptr<image_transport::ImageTransport>;
 using ITSubscriberPtr = std::shared_ptr<image_transport::Subscriber>;
 using TFBufferPtr = std::shared_ptr<tf2_ros::Buffer>;

package.xml

@@ -17,16 +17,19 @@
     <build_depend>tf2_ros</build_depend>
     <build_depend>rmcl_msgs</build_depend>
     <build_depend>image_transport</build_depend>
+    <build_depend>visualization_msgs</build_depend>
     <build_export_depend>roscpp</build_export_depend>
     <build_export_depend>geometry_msgs</build_export_depend>
     <build_export_depend>sensor_msgs</build_export_depend>
     <build_export_depend>tf2_ros</build_export_depend>
     <build_export_depend>rmcl_msgs</build_export_depend>
     <build_export_depend>image_transport</build_export_depend>
+    <build_export_depend>visualization_msgs</build_export_depend>
     <exec_depend>roscpp</exec_depend>
     <exec_depend>geometry_msgs</exec_depend>
     <exec_depend>sensor_msgs</exec_depend>
     <exec_depend>tf2_ros</exec_depend>
     <exec_depend>rmcl_msgs</exec_depend>
     <exec_depend>image_transport</exec_depend>
+    <exec_depend>visualization_msgs</exec_depend>
 </package>

src/nodes/micp_localization.cpp

@@ -45,6 +45,7 @@ std::mutex                      T_base_odom_mutex;
 Transform Tbo;
 
 bool invert_tf = false;
+bool correction_disabled = false;
 
 
 Transform initial_pose_offset;
@@ -239,7 +240,12 @@ void correctOnce()
     // pose == Tbm
     // Tom = Tbm * Tob
     // 
-    Tom = poses[0] * ~Tbo;
+
+    // apply actual correction as Tom
+    if(!correction_disabled)
+    {
+        Tom = poses[0] * ~Tbo;
+    }
 }
 
 // Storing Pose information globally
@@ -420,6 +426,7 @@ int main(int argc, char** argv)
     ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("pose", 1, poseCB);
     ros::Subscriber pose_wc_sub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("pose_wc", 1, poseWcCB);
 
+
     // CORRECTION THREAD
     stop_correction_thread = false;
     correction_thread = std::thread([corr_rate_max, print_corr_rate]()
@@ -430,6 +437,9 @@ int main(int argc, char** argv)
         // minimum duration for one loop
         double el_min = 1.0 / corr_rate_max;
 
+        // reactivate cuda context if required
+        micp->useInThisThread();
+
         while(!stop_correction_thread)
         {
             sw();

src/rmcl/correction/MICP.cpp

@@ -766,6 +766,7 @@ bool MICP::loadSensor(std::string sensor_name, XmlRpc::XmlRpcValue sensor_params
 
     // connect sensor to ROS
     sensor->nh = m_nh;
+    sensor->nh_p = m_nh_p;
     sensor->tf_buffer = m_tf_buffer; 
     sensor->connect();