diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index 6ad403e33cfc355be7ae8b63078ed600f6aba49a..d80c10bfbbc4edadc403e0038c2e5ae8303f9886 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -24,16 +24,17 @@ CaptureBase::~CaptureBase()
std::cout << "destructed -C" << id() << std::endl;
}
-void CaptureBase::process()
-{
- // Call all processors assigned to the sensor that captured this data
- auto cap = shared_from_this();
- auto sen = sensor_ptr_.lock();
- if (sen)
- for (auto prc : sen->getProcessorList())
- prc->process(cap);
-}
+//void CaptureBase::process()
+//{
+// // Call all processors assigned to the sensor that captured this data
+// auto cap = shared_from_this();
+// auto sen = sensor_ptr_.lock();
+// if (sen)
+// for (auto prc : sen->getProcessorList())
+// prc->process(cap);
+//}
+//<<<<<<< HEAD
void CaptureBase::remove()
{
if (!is_removing_)
@@ -59,6 +60,16 @@ void CaptureBase::remove()
std::cout << "Removed C" << id() << std::endl;
}
}
+//=======
+//void CaptureBase::process()
+//{
+// // Call all processors assigned to the sensor that captured this data
+// for (auto processor_iter = sensor_ptr_->getProcessorListPtr()->begin(); processor_iter != sensor_ptr_->getProcessorListPtr()->end(); ++processor_iter)
+// {
+// (*processor_iter)->process(this);
+// }
+//}
+//>>>>>>> capture_passby_sensor
void CaptureBase::addFeatureList(FeatureBaseList& _new_ft_list)
{
diff --git a/src/capture_base.h b/src/capture_base.h
index bc04bd822588f92d55256a8c1c1ddaac1074aa7f..2ea42758c7f20d84144d696e8adf883e419d5c17 100644
--- a/src/capture_base.h
+++ b/src/capture_base.h
@@ -36,7 +36,16 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
public:
- CaptureBase(const std::string& _type, const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
+//<<<<<<< HEAD
+// CaptureBase(const std::string& _type, const TimeStamp& _ts, SensorBasePtr _sensor_ptr);
+//=======
+ CaptureBase(const std::string& _type, const TimeStamp& _ts, SensorBasePtr _sensor_ptr = nullptr);
+
+// /** \brief Default destructor (not recommended)
+// *
+// * Default destructor (please use destruct() instead of delete for guaranteeing the wolf tree integrity)
+// **/
+//>>>>>>> capture_passby_sensor
virtual ~CaptureBase();
void remove();
@@ -58,10 +67,21 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void getConstraintList(ConstraintBaseList& _ctr_list);
SensorBasePtr getSensorPtr() const;
+//<<<<<<< HEAD
StateBlockPtr getSensorPPtr() const;
StateBlockPtr getSensorOPtr() const;
- virtual void process();
+// virtual void process();
+//=======
+ virtual void setSensorPtr(const SensorBasePtr sensor_ptr);
+
+// StateBlock* getSensorPPtr() const;
+// StateBlock* getSensorOPtr() const;
+//
+// /** \brief Call all the processors for this Capture
+// */
+// //virtual void process();
+//>>>>>>> capture_passby_sensor
};
@@ -151,6 +171,11 @@ inline SensorBasePtr CaptureBase::getSensorPtr() const
return sensor_ptr_.lock();
}
+inline void CaptureBase::setSensorPtr(const SensorBasePtr sensor_ptr)
+{
+ sensor_ptr_ = sensor_ptr;
+}
+
inline void CaptureBase::setTimeStamp(const TimeStamp& _ts)
{
time_stamp_ = _ts;
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index 326784005f4231cc9fa6a69b7ddb5e67fbb85a38..6c0a406a43efe1ef5af170e936dd0f387c923990 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -28,6 +28,8 @@ class CaptureLaser2D : public CaptureBase
laserscanutils::LaserScan& getScan();
+ void setSensorPtr(const SensorBasePtr sensor_ptr);
+
private:
SensorLaser2D::Ptr laser_ptr_; //specific pointer to sensor laser 2D object
laserscanutils::LaserScan scan_;
@@ -39,6 +41,12 @@ inline laserscanutils::LaserScan& CaptureLaser2D::getScan()
return scan_;
}
+inline void CaptureLaser2D::setSensorPtr(const SensorBasePtr sensor_ptr)
+{
+ CaptureBase::setSensorPtr(sensor_ptr);
+ laser_ptr_ = std::static_pointer_cast<SensorLaser2D>(sensor_ptr);
+}
+
} // namespace wolf
#endif /* CAPTURE_LASER_2D_H_ */
diff --git a/src/examples/test_ROI_ORB.cpp b/src/examples/test_ROI_ORB.cpp
index b815f59231b22a986f66782125903a3f7f8e6e69..ab15fbf7d2f0d9b27949cd558fe65b604407f82b 100644
--- a/src/examples/test_ROI_ORB.cpp
+++ b/src/examples/test_ROI_ORB.cpp
@@ -41,7 +41,7 @@ int main(int argc, char** argv)
cv::Feature2D* detector_descriptor_ptr_;
- cv::DescriptorMatcher* matcher_ptr_;
+// cv::DescriptorMatcher* matcher_ptr_;
unsigned int nfeatures = 20;
float scaleFactor = 1.2;
@@ -52,7 +52,7 @@ int main(int argc, char** argv)
unsigned int scoreType = 0; //#enum { kBytes = 32, HARRIS_SCORE=0, FAST_SCORE=1 };
unsigned int patchSize = 31;
- unsigned int fastThreshold = 20;
+// unsigned int fastThreshold = 20;
unsigned int roi_width = 50;
unsigned int roi_heigth = 50;
@@ -67,16 +67,16 @@ int main(int argc, char** argv)
patchSize);//,
// fastThreshold);
- unsigned int nominal_pattern_radius = 0;
- unsigned int pattern_radius = (unsigned int)( (nominal_pattern_radius) * pow(scaleFactor, nlevels-1));
+// unsigned int nominal_pattern_radius = 0;
+// unsigned int pattern_radius = (unsigned int)( (nominal_pattern_radius) * pow(scaleFactor, nlevels-1));
// std::cout << "nominal pattern radius: " << _dd_params->nominal_pattern_radius << std::endl;
// std::cout << "scale factor: " << params_orb->scaleFactor << std::endl;
// std::cout << "nlevels: " << params_orb->nlevels << std::endl;
- unsigned int size_bits = detector_descriptor_ptr_->descriptorSize() * 8;
+// unsigned int size_bits = detector_descriptor_ptr_->descriptorSize() * 8;
- matcher_ptr_ = new cv::BFMatcher(6);
+// matcher_ptr_ = new cv::BFMatcher(6);
diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp
index b18c60ff31b6691128ee64501755e26b0d5c67c9..fd525e9246542c386ac5ac73d70177cb417efcfc 100644
--- a/src/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -183,9 +183,16 @@ int main(int argc, char** argv)
// prc_image->process(capture_image_ptr);
// Preferred method with factory objects:
+//<<<<<<< HEAD
image_ptr = make_shared<CaptureImage>(t, camera_ptr, frame[f % buffer_size]);
- image_ptr->process();
-
+// image_ptr->process();
+
+//=======
+// image_ptr = new CaptureImage(t, camera_ptr, frame[f % buffer_size]);
+ //image_ptr->process();
+ camera_ptr->addCapture(image_ptr);
+ //cv::imshow("test",frame[f % buffer_size]);
+//>>>>>>> capture_passby_sensor
std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
wolf_problem_->print();
diff --git a/src/examples/test_mpu.cpp b/src/examples/test_mpu.cpp
index 6919a300d9c9a0493c255cceb1a2bcef8ad056be..e041ed0858025bf4fc3ffc5eb773b375fb60b853 100644
--- a/src/examples/test_mpu.cpp
+++ b/src/examples/test_mpu.cpp
@@ -158,7 +158,8 @@ int main(int argc, char** argv)
imu_ptr->setTimeStamp(t);
// process data in capture
- imu_ptr ->process();
+ sensor_ptr->addCapture(imu_ptr);
+// imu_ptr ->process();
#ifdef DEBUG_RESULTS
diff --git a/src/examples/test_opencv.cpp b/src/examples/test_opencv.cpp
index fac549426379fdda068e44a162cb88b97f24a991..b1f6b60d3a275cd38d87bd2bebf1ecd6c736b395 100644
--- a/src/examples/test_opencv.cpp
+++ b/src/examples/test_opencv.cpp
@@ -88,7 +88,7 @@ int main(int argc, char** argv)
std::vector<cv::Point2f> inliers_1, inliers_2;
cv::Mat img_matches;
cv::Mat img_scaled;
- double scale = 1;//0.875;
+// double scale = 1;//0.875;
unsigned int f = 0;
capture >> image_buffer[f % buffer_size];
diff --git a/src/examples/test_processor_image_landmark.cpp b/src/examples/test_processor_image_landmark.cpp
index 7f4f5829cb3671f00c0b73868f6dbc0b31edd881..2ac848d5dd2b0217d316ec6fcede7c969720260b 100644
--- a/src/examples/test_processor_image_landmark.cpp
+++ b/src/examples/test_processor_image_landmark.cpp
@@ -197,7 +197,8 @@ int main(int argc, char** argv)
}
cap_odo->setData(data);
- cap_odo->process();
+ sen_odo_ptr->addCapture(cap_odo);
+// cap_odo->process();
wolf_problem_ptr_->print();
@@ -211,7 +212,8 @@ int main(int argc, char** argv)
image_ptr = std::make_shared<CaptureImage>(t, camera_ptr, frame[f % buffer_size]);
/* process */
- image_ptr->process();
+ //image_ptr->process();
+ camera_ptr->addCapture(image_ptr);
std::cout << "Time: " << ((double) clock() - t1) / CLOCKS_PER_SEC << "s" << std::endl;
diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
index b43e0e708b4a16120fc5fb82c83bbdaea83ea3fe..3aa39a1399f7cd0614c6940ba6d59ae8633566a6 100644
--- a/src/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -118,7 +118,8 @@ int main(int argc, char** argv)
imu_ptr->setTimeStamp(t);
// process data in capture
- imu_ptr ->process();
+ //imu_ptr ->process();
+ sensor_ptr->addCapture(imu_ptr);
#ifdef DEBUG_RESULTS
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index 89646f3b588fa5b8b2555813ce1f408744fb3587..861443f728e26745b4b3349dd5f99d42cd29396d 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -73,7 +73,9 @@ int main (int argc, char** argv)
{
cap_odo->setTimeStamp(t);
cap_odo->setData(data);
- cap_odo->process();
+
+ sen->addCapture(cap_odo);
+// cap_odo->process();
cout << "t: " << std::setprecision(2) << t.get() << " \t x = ( " << problem->getCurrentState().transpose() << ")" << endl;
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 0d7154d19d64a39cc6dba37ebc5dac3feedea3d3..bd0464cc5547226ed08fbe922ee7e9082f905b97 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -121,10 +121,13 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
HardwareBasePtr getHardwarePtr();
void setHardwarePtr(const HardwareBasePtr _hw_ptr);
+ bool addCapture(const CaptureBasePtr capture_ptr);
+
};
}
+#include "capture_base.h"
#include "processor_base.h"
#include "hardware_base.h"
@@ -239,6 +242,18 @@ inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
hardware_ptr_ = _hw_ptr;
}
+inline bool SensorBase::addCapture(const CaptureBasePtr capture_ptr)
+{
+ capture_ptr->setSensorPtr(shared_from_this());
+
+ for (const auto processor : processor_list_)
+ {
+ processor->process(capture_ptr);
+ }
+
+ return true;
+}
+
} // namespace wolf
#endif