Skip to content
Snippets Groups Projects
Commit d3f0cc39 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Update gtest_processor_bundle_adjustment.cpp

parent 73fa2c15
No related branches found
No related tags found
2 merge requests!24After 2nd RAL submission,!20Resolve "follow core:#313"
Hide whitespace changes
Inline Side-by-side
test/gtest_processor_bundle_adjustment.cpp
+ 36
− 29
View file @ d3f0cc39
...@@ -26,7 +26,11 @@ class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment ...@@ -26,7 +26,11 @@ class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
{ {
public: public:
ProcessorBundleAdjustmentDummy(ParamsProcessorBundleAdjustmentPtr& _params_bundle_adjustment):ProcessorBundleAdjustment(_params_bundle_adjustment){} ProcessorBundleAdjustmentDummy(ParamsProcessorBundleAdjustmentPtr& _params_bundle_adjustment):
ProcessorBundleAdjustment(_params_bundle_adjustment)
{
}
void setLast(CaptureImagePtr _last_ptr) void setLast(CaptureImagePtr _last_ptr)
{ {
...@@ -68,11 +72,9 @@ class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment ...@@ -68,11 +72,9 @@ class ProcessorBundleAdjustmentDummy : public ProcessorBundleAdjustment
return matches_last_from_incoming_; return matches_last_from_incoming_;
} }
CaptureImagePtr createCaptureImage(std::string _path, SensorCameraPtr _sensor, bool detectAndDescript = false) CaptureImagePtr createCaptureImage(TimeStamp _ts, std::string _path, SensorCameraPtr _sensor, bool detectAndDescript = false)
{ {
const double time = 0.0; CaptureImagePtr im = std::make_shared<CaptureImage>(_ts, _sensor, cv::imread(_path));
TimeStamp ts(time);
CaptureImagePtr im = std::make_shared<CaptureImage>(ts, _sensor, cv::imread(_path));
if (detectAndDescript){ if (detectAndDescript){
// Detect KeyPoints // Detect KeyPoints
im->keypoints_ = det_ptr_->detect(im->getImage()); im->keypoints_ = det_ptr_->detect(im->getImage());
...@@ -138,10 +140,10 @@ TEST(ProcessorBundleAdjustment, preProcess) ...@@ -138,10 +140,10 @@ TEST(ProcessorBundleAdjustment, preProcess)
auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params); auto proc_dummy = std::make_shared<ProcessorBundleAdjustmentDummy>(params);
// Put an image on incoming_ptr_ // Put an image on incoming_ptr_
CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr); CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(0, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr); proc_dummy->setInc(image_inc_ptr);
// Put an image on last_ptr_ // Put an image on last_ptr_
CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true); CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(1, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
proc_dummy->setLast(image_last_ptr); proc_dummy->setLast(image_last_ptr);
// demo dpreProcess // demo dpreProcess
proc_dummy->preProcess(); proc_dummy->preProcess();
...@@ -169,7 +171,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures) ...@@ -169,7 +171,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>(); FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
// Put an image on last_ptr_ // Put an image on last_ptr_
CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true); CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(0, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
ASSERT_NE(image_last_ptr->keypoints_.size(), 0); ASSERT_NE(image_last_ptr->keypoints_.size(), 0);
proc_dummy->setLast(image_last_ptr); proc_dummy->setLast(image_last_ptr);
...@@ -178,7 +180,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures) ...@@ -178,7 +180,7 @@ TEST(ProcessorBundleAdjustment, detectNewFeatures)
ASSERT_EQ(num, 0); ASSERT_EQ(num, 0);
// Put an image on incoming_ptr_ // Put an image on incoming_ptr_
CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr); CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(1, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr); proc_dummy->setInc(image_inc_ptr);
// demo detectNewFeatures // demo detectNewFeatures
...@@ -212,9 +214,9 @@ TEST(ProcessorBundleAdjustment, trackFeatures) ...@@ -212,9 +214,9 @@ TEST(ProcessorBundleAdjustment, trackFeatures)
//fill feat_last list //fill feat_last list
FeatureBasePtrList feat_list = std::list<FeatureBasePtr>(); FeatureBasePtrList feat_list = std::list<FeatureBasePtr>();
CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true); CaptureImagePtr image_last_ptr = proc_dummy->createCaptureImage(0, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr, true);
proc_dummy->setLast(image_last_ptr); proc_dummy->setLast(image_last_ptr);
CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr); CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(1, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", nullptr);
proc_dummy->setInc(image_inc_ptr); proc_dummy->setInc(image_inc_ptr);
proc_dummy->preProcess(); proc_dummy->preProcess();
CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast()); CaptureImagePtr last = std::static_pointer_cast<CaptureImage>(proc_dummy->getLast());
...@@ -282,10 +284,10 @@ TEST(ProcessorBundleAdjustment, process) ...@@ -282,10 +284,10 @@ TEST(ProcessorBundleAdjustment, process)
ProblemPtr problem = Problem::create("PO", 3); ProblemPtr problem = Problem::create("PO", 3);
// Install camera // Install camera
ParamsSensorCameraPtr intr = std::make_shared<ParamsSensorCamera>(); // TODO init params or read from YAML ParamsSensorCameraPtr intr = std::make_shared<ParamsSensorCamera>();
intr->pinhole_model_raw = Eigen::Vector4d(0,0,1,1); //TODO: initialize intr->pinhole_model_raw = Eigen::Vector4d(640, 480, 640, 640);
intr->width = 640; intr->width = 1280;
intr->height = 480; intr->height = 960;
SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("SensorCamera", "camera", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), intr)); SensorCameraPtr sens_cam = std::static_pointer_cast<SensorCamera>(problem->installSensor("SensorCamera", "camera", (Eigen::Vector7d() << 0,0,0, 0,0,0,1).finished(), intr));
// Install processor // Install processor
...@@ -303,38 +305,42 @@ TEST(ProcessorBundleAdjustment, process) ...@@ -303,38 +305,42 @@ TEST(ProcessorBundleAdjustment, process)
auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc); auto proc_dummy = std::static_pointer_cast<ProcessorBundleAdjustmentDummy>(proc);
//1ST TIME //1ST TIME
CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam); CaptureImagePtr image_inc_ptr = proc_dummy->createCaptureImage(1, wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam);
image_inc_ptr->process(); image_inc_ptr->process();
// proc_dummy->process(image_inc_ptr);
problem->print(4,0,1,0);
ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), 0); ASSERT_EQ(proc_dummy->getTrackMatrix().numTracks(), 0);
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 0); ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 0);
//2ND TIME //2ND TIME
CaptureImagePtr image_inc_ptr2 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam); CaptureImagePtr image_inc_ptr2 = proc_dummy->createCaptureImage(2, wolf_vision_root + "/demos/demo_gazebo_x00cm_y-10cm.jpg", sens_cam);
// proc_dummy->setInc(image_inc_ptr2);
// proc_dummy->preProcess();
// proc_dummy->getProcessKnown();
// proc_dummy->getProcessNew(params->max_new_features);
// proc_dummy->establishFactors();
image_inc_ptr2->process(); image_inc_ptr2->process();
// proc_dummy->process(image_inc_ptr2);
problem->print(4,0,1,0);
ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features); ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
ASSERT_NE(problem->getMap(), nullptr); ASSERT_NE(problem->getMap(), nullptr);
ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), true);
//3RD TIME -- RUNNING //3RD TIME -- RUNNING
CaptureImagePtr image_inc_ptr3 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam); CaptureImagePtr image_inc_ptr3 = proc_dummy->createCaptureImage(3, wolf_vision_root + "/demos/demo_gazebo_x00cm_y-20cm.jpg", sens_cam);
assert(proc_dummy->getOrigin()!=nullptr); assert(proc_dummy->getOrigin()!=nullptr);
assert(proc_dummy->getLast()!= nullptr && proc_dummy->getLast()!=proc_dummy->getOrigin()); assert(proc_dummy->getLast()!= nullptr && proc_dummy->getLast()!=proc_dummy->getOrigin());
image_inc_ptr3->process(); image_inc_ptr3->process();
// proc_dummy->process(image_inc_ptr3);
problem->print(4,0,1,0);
ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features); ASSERT_LE(proc_dummy->getTrackMatrix().numTracks(), params->max_new_features);
//4TH TIME -- RUNNING //4TH TIME -- RUNNING
CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(wolf_vision_root + "/demos/demo_gazebo_x00cm_y00cm.jpg", sens_cam); CaptureImagePtr image_inc_ptr4 = proc_dummy->createCaptureImage(4, wolf_vision_root + "/demos/demo_gazebo_x-10cm_y-20cm.jpg", sens_cam);
image_inc_ptr4->process(); image_inc_ptr4->process();
// proc_dummy->process(image_inc_ptr4);
problem->print(4,0,1,0);
ASSERT_LE(image_inc_ptr4->getFeatureList().size(), params->max_new_features); ASSERT_LE(image_inc_ptr4->getFeatureList().size(), params->max_new_features);
} }
...@@ -353,6 +359,7 @@ TEST(ProcessorBundleAdjustment, processNew) ...@@ -353,6 +359,7 @@ TEST(ProcessorBundleAdjustment, processNew)
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
// ::testing::GTEST_FLAG(filter) = "ProcessorBundleAdjustment.process";
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment