diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp index ede5bd88523c313fd3bbd4bac9a8d2efaada1f54..fd09248ade5284fc0ee24977c323ecb1e30fcd02 100644 --- a/src/capture_laser_2D.cpp +++ b/src/capture_laser_2D.cpp @@ -97,7 +97,7 @@ void CaptureLaser2D::establishConstraints() for (auto feature_it = getFeatureListPtr()->begin(); feature_it != getFeatureListPtr()->end(); feature_it++ ) { double max_distance_matching2 = 0.5; //TODO: max_distance_matching depending on localization and landmarks uncertainty - double max_theta_matching = 0.2; //TODO: max_theta_matching depending on localization and landmarks uncertainty + double max_theta_matching = M_PI / 8; //TODO: max_theta_matching depending on localization and landmarks uncertainty //Find the closest landmark to the feature LandmarkCorner2D* correspondent_landmark = nullptr; @@ -127,18 +127,18 @@ void CaptureLaser2D::establishConstraints() { if (theta_distance < max_theta_matching) { - //std::cout << "Position & orientation near landmark found: " << (*landmark_it)->nodeId() << std::endl; - //std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl; +// std::cout << "Position & orientation near landmark found: " << (*landmark_it)->nodeId() << std::endl; +// std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl; correspondent_landmark = (LandmarkCorner2D*)(*landmark_it); min_distance2 = distance2; } else { -// std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl; -// std::cout << "global position:" << feature_global_position.transpose() << " orientation:" << feature_global_orientation << std::endl; -// std::cout << "Landmark with near position but wrong orientation: " << (*landmark_it)->nodeId() << std::endl; -// std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl; + std::cout << "Feature: " << (*feature_it)->nodeId() << std::endl; + std::cout << "global position:" << feature_global_position.transpose() << " orientation:" << feature_global_orientation << std::endl; + std::cout << "Landmark with near position but wrong orientation: " << (*landmark_it)->nodeId() << std::endl; + std::cout << "global position:" << landmark_position.transpose() << " orientation:" << landmark_orientation << std::endl; } } } diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index a127da1d7164f265db97e04c45bf784898f9c691..31bc5d1003c034d99e171a1f03f5d5705572ae4b 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -112,6 +112,7 @@ class WolfManager SensorBase* sensor_prior_; unsigned int window_size_; FrameBaseIter first_window_frame_; + CaptureRelative* last_capture_relative_; public: WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length, const Eigen::VectorXs& _init_frame, const unsigned int& _w_size=10) : @@ -151,6 +152,7 @@ class WolfManager sensor_prior_, Eigen::Vector2s::Zero(), Eigen::Matrix2s::Zero())); + last_capture_relative_ = (CaptureRelative*)(problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->front()); } void addCapture(CaptureBase* _capture) @@ -171,21 +173,22 @@ class WolfManager if (new_capture->getSensorPtr() == sensor_prior_) { // FIND PREVIOUS RELATIVE CAPTURE - CaptureRelative* previous_relative_capture = nullptr; - for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++) - { - //std::cout << "capture: " << (*capture_it)->nodeId() << std::endl; - if ((*capture_it)->getSensorPtr() == sensor_prior_) - { - previous_relative_capture = (CaptureRelative*)(*capture_it); - break; - } - } +// CaptureRelative* previous_relative_capture = nullptr; +// for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++) +// { +// //std::cout << "capture: " << (*capture_it)->nodeId() << std::endl; +// if ((*capture_it)->getSensorPtr() == sensor_prior_) +// { +// previous_relative_capture = (CaptureRelative*)(*capture_it); +// break; +// } +// } // INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR //std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl; - previous_relative_capture->integrateCapture((CaptureRelative*)(new_capture)); - Eigen::VectorXs prior = previous_relative_capture->computePrior(); +// previous_relative_capture->integrateCapture((CaptureRelative*)(new_capture)); +// Eigen::VectorXs prior = previous_relative_capture->computePrior(); + last_capture_relative_->integrateCapture((CaptureRelative*)(new_capture)); // NEW KEY FRAME (if enough time from last frame) //std::cout << "new TimeStamp - last Frame TimeStamp = " << new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() << std::endl; @@ -196,7 +199,7 @@ class WolfManager // NEW FRAME //std::cout << "new frame" << std::endl; - createFrame(prior, new_capture->getTimeStamp()); + createFrame(last_capture_relative_->computePrior(), new_capture->getTimeStamp()); // COMPUTE PREVIOUS FRAME CAPTURES //std::cout << "compute prev frame" << std::endl; @@ -237,7 +240,7 @@ class WolfManager Eigen::VectorXs getVehiclePose() { - return Eigen::Map<Eigen::VectorXs>(problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr()->getPtr(), use_complex_angles_ ? 4 : 3); + return last_capture_relative_->computePrior(); } WolfProblem* getProblemPtr()