Add capture type check for various processors

src/processor/processor_bundle_adjustment.cpp

@@ -70,7 +70,10 @@ void ProcessorBundleAdjustment::preProcess()
 {
     // This method implements all Vision algorithms concerning OpenCV, so wolf algorithm only has to manage the data obtained
     // Get Capture
-    capture_image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_);
+    capture_image_incoming_ = std::dynamic_pointer_cast<CaptureImage>(incoming_ptr_);
+    assert(capture_image_incoming_ != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImage").c_str());
+    // capture_image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_);
+
     // Detect KeyPoints
     capture_image_incoming_->keypoints_ = det_ptr_->detect(capture_image_incoming_->getImage());
 

src/processor/processor_tracker_feature_image.cpp

@@ -59,7 +59,9 @@ void ProcessorTrackerFeatureImage::configure(SensorBasePtr _sensor)
 
 void ProcessorTrackerFeatureImage::preProcess()
 {
-    image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_)->getImage();
+    auto incoming_ptr = std::dynamic_pointer_cast<CaptureImage>(incoming_ptr_);
+    assert(incoming_ptr != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImage").c_str());
+    image_incoming_ = incoming_ptr->getImage();
 
     active_search_ptr_->renew();
 

src/processor/processor_tracker_landmark_image.cpp

@@ -72,8 +72,9 @@ void ProcessorTrackerLandmarkImage::configure(SensorBasePtr _sensor)
 
 void ProcessorTrackerLandmarkImage::preProcess()
 {
-    image_incoming_ = std::static_pointer_cast<CaptureImage>(incoming_ptr_)->getImage();
-
+    auto incoming_ptr = std::dynamic_pointer_cast<CaptureImage>(incoming_ptr_);
+    assert(incoming_ptr != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImage").c_str());
+    image_incoming_ = incoming_ptr->getImage();
     active_search_ptr_->renew();
 
     detector_roi_.clear();