diff --git a/CMakeLists.txt b/CMakeLists.txt index 0242a96b77867854aead97822c282920234509fd..9c71d5edb9d94e4b0fd4d475ad675605da295374 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,18 +27,14 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.") SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT") SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT") -#Set compiler according C++11 support +#Set compiler according C++14 support include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif(COMPILER_SUPPORTS_CXX0X) - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) +if(COMPILER_SUPPORTS_CXX14) + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") else() - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") endif() if(UNIX) diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt index 93d17fbe0befb99c22622c7f48f5fd67511467df..191653f92527669418d058a7c2a7f6fa399d0b73 100644 --- a/demos/CMakeLists.txt +++ b/demos/CMakeLists.txt @@ -1,26 +1,10 @@ INCLUDE_DIRECTORIES( ${CMAKE_CURRENT_SOURCE_DIR} - ${catkin_INCLUDE_DIRS} ) -FIND_PACKAGE(catkin REQUIRED COMPONENTS - rosbag - rosconsole - roscpp - roslib - sensor_msgs - std_msgs - geometry_msgs - tf - wolf_ros_objectslam - wolf_ros_node -) FIND_PACKAGE(wolfimu REQUIRED) INCLUDE_DIRECTORIES( - ${rosbag_INCLUDE_DIRS} - ${wolf_ros_objectslam_INCLUDE_DIRS} - ${std_msgs_INCLUDE_DIRS} ${wolfimu_INCLUDE_DIRS} ) @@ -29,22 +13,22 @@ ADD_EXECUTABLE(demo_toy_pbe demo_toy_pbe.cpp) TARGET_LINK_LIBRARIES(demo_toy_pbe ${wolfcore_LIBRARIES} ${PLUGIN_NAME} - ${catkin_LIBRARIES} -) - -ADD_EXECUTABLE(cosyslam cosyslam.cpp) -TARGET_LINK_LIBRARIES(cosyslam - ${wolfcore_LIBRARIES} - ${PLUGIN_NAME} - ${catkin_LIBRARIES} + # ${catkin_LIBRARIES} ) -ADD_EXECUTABLE(cosyslam_imu cosyslam_imu.cpp) -TARGET_LINK_LIBRARIES(cosyslam_imu - ${wolfcore_LIBRARIES} - ${PLUGIN_NAME} - ${catkin_LIBRARIES} - ${wolfimu_LIBRARIES} -) +# ADD_EXECUTABLE(cosyslam cosyslam.cpp) +# TARGET_LINK_LIBRARIES(cosyslam +# ${wolfcore_LIBRARIES} +# ${PLUGIN_NAME} +# ${catkin_LIBRARIES} +# ) + +# ADD_EXECUTABLE(cosyslam_imu cosyslam_imu.cpp) +# TARGET_LINK_LIBRARIES(cosyslam_imu +# ${wolfcore_LIBRARIES} +# ${PLUGIN_NAME} +# ${catkin_LIBRARIES} +# ${wolfimu_LIBRARIES} +# ) diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp index fcbd5093572b46bf9d9b6751e5b3e6136f3a848c..2a4de756d05d8672edeb08ba534a7fdece72b00c 100644 --- a/demos/cosyslam.cpp +++ b/demos/cosyslam.cpp @@ -84,7 +84,7 @@ int main() ObjectDetections dets; VectorComposite x0("PO", {Vector3d::Zero(), Quaterniond::Identity().coeffs()}); VectorComposite sig0("PO", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1}); - FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1); + FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0); // Variables init Matrix6d cov; diff --git a/demos/cosyslam_imu.cpp b/demos/cosyslam_imu.cpp index 3bacab812af0eda3aa268a67c9e00ff2dbb7fc66..d783e93fe6c5be14199800e865f87706b848111d 100644 --- a/demos/cosyslam_imu.cpp +++ b/demos/cosyslam_imu.cpp @@ -157,7 +157,7 @@ int main() VectorComposite x0("POV", { Vector3d::Zero(), q_init, Vector3d::Zero()}); VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1, Vector3d::Ones()*0.01}); - FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1); + FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0); foreach(rosbag::MessageInstance const m, view) { diff --git a/demos/demo_toy_pbe.cpp b/demos/demo_toy_pbe.cpp index b8941930d607e418f9827416b471016a026c6c1f..57b9b7bc7b8bca93282251e26c8163ea3b24d0e7 100644 --- a/demos/demo_toy_pbe.cpp +++ b/demos/demo_toy_pbe.cpp @@ -85,7 +85,7 @@ int main() { VectorComposite x0("PO", {Vector3d(0,0,0), Quaterniond::Identity().coeffs()}); VectorComposite sig0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)}); - FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1); + FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0); // define measurements according to "detections" represented in the graph above Matrix6d cov = Matrix6d::Identity(); diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp index c3d6dae0f40144640a339c044dd79f63005d9cfa..22e324bd6ced20ba2ebcebaf68792656ed8ce0f3 100644 --- a/src/processor/processor_tracker_landmark_object.cpp +++ b/src/processor/processor_tracker_landmark_object.cpp @@ -1,6 +1,6 @@ #include "core/math/rotations.h" #include <core/factor/factor_distance_3d.h> -#include "core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h" +#include "core/factor/factor_relative_pose_3d_with_extrinsics.h" // Wolf object plugin #include "objectslam/processor/processor_tracker_landmark_object.h" @@ -85,14 +85,11 @@ FactorBasePtr ProcessorTrackerLandmarkObject::emplaceFactor(FeatureBasePtr _feat // A keyframe is voted keyframe_voted_ = true; - return FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(_feature_ptr, - getSensor(), - getLast()->getFrame(), - std::static_pointer_cast<LandmarkObject>(_landmark_ptr), - std::static_pointer_cast<FeatureObject> (_feature_ptr ), - shared_from_this(), - params_->apply_loss_function, - FAC_ACTIVE); + return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature_ptr, + _feature_ptr, + _landmark_ptr, + shared_from_this(), + params_->apply_loss_function); } LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _feature_ptr) @@ -264,9 +261,6 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr } - - - FeatureBasePtrList ProcessorTrackerLandmarkObject::getIncomingDetections() const { return detections_incoming_; @@ -278,7 +272,6 @@ FeatureBasePtrList ProcessorTrackerLandmarkObject::getLastDetections() const } - void ProcessorTrackerLandmarkObject::advanceDerived() { ProcessorTrackerLandmark::advanceDerived(); @@ -287,153 +280,10 @@ void ProcessorTrackerLandmarkObject::advanceDerived() void ProcessorTrackerLandmarkObject::resetDerived() { - // Add 3d distance constraint between 2 frames - if (getProblem()->getProcessorIsMotionMap().empty() && add_3d_cstr_){ - if ((getOrigin() != nullptr) && - (getOrigin()->getFrame() != nullptr) && - (getOrigin() != getLast()) && - (getOrigin()->getFrame() != getLast()->getFrame()) - ) - { - FrameBasePtr ori_frame = getOrigin()->getFrame(); - Eigen::Vector1d dist_meas; dist_meas << 0.0; - double dist_std = 0.5; - Eigen::Matrix1d cov0(dist_std*dist_std); - - auto capt3d = CaptureBase::emplace<CaptureBase>(getLast()->getFrame(),"Dist",getLast()->getTimeStamp()); - auto feat_dist = FeatureBase::emplace<FeatureBase>(capt3d, "Dist", dist_meas, cov0); - auto cstr = FactorBase::emplace<FactorDistance3d>(feat_dist, feat_dist, ori_frame, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE); - } - } - - if (getProblem()->getProcessorIsMotionMap().empty() && reestimate_last_frame_){ - reestimateLastFrame(); - } - ProcessorTrackerLandmark::resetDerived(); detections_last_ = std::move(detections_incoming_); } -void ProcessorTrackerLandmarkObject::reestimateLastFrame(){ - // !!!!!!!!!!!!!!!!!!!!!!!!!!! - // !! TOO COMPLEX: to rewrite - // !!!!!!!!!!!!!!!!!!!!!!!!!!! - - - // // Rewrite the last frame state and landmark state initialised during last frame creation to account - // // for a better estimate of the current state using the last landmark detection. - // // Otherwise, default behaviour is to take the last KF as an initialization of the new KF which can lead - // // to the solver finding local minima - - // When called for the first time, no feature list initialized in ori/last(?) - if (n_reset_ < 1){ - n_reset_ += 1; - return; - } - - // // A TESTER - // // (getOrigin() != nullptr) - - // // Retrieve camera extrinsics - // Eigen::Quaterniond last_q_cam(getSensor()->getO()->getState().data()); - // Eigen::Isometry3d last_M_cam = Eigen::Translation3d(getSensor()->getP()->getState()) * last_q_cam; - - // // get features list of KF linked to origin capture and last capture - // FeatureBasePtrList ori_feature_list = getOrigin()->getFeatureList(); - // FeatureBasePtrList last_feature_list = getLast()->getFeatureList(); - - // if (last_feature_list.size() == 0 || ori_feature_list.size() == 0){ - // return; - // } - - // // Among landmarks detected in origin and last, find the one that has the smallest error ratio (best confidence) - // double lowest_ration = 1; // rep_error1/rep_error2 cannot be higher than 1 - // FeatureObjectPtr best_feature; - // bool useable_feature = false; - // for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){ - // FeatureObjectPtr last_feat_ptr = std::static_pointer_cast<FeatureObject>(*it_last); - // for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){ - // FeatureObjectPtr ori_feat_ptr = std::static_pointer_cast<FeatureObject>(*it_ori); - // if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){ - // double ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2(); - // //if (ratio < lowest_ration){ - // if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){ - // useable_feature = true; - // lowest_ration = ratio; - // best_feature = last_feat_ptr; - // } - // } - // } - // } - // // If there is no common feature between the two images, the continuity is broken and - // // nothing can be estimated. In the case of objslam alone, this result in a broken factor map - // if (!useable_feature){ - // return; - // } - - // // std::cout << "Best feature id after: " << best_feature->getTagId() << std::endl; - // // Retrieve cam to landmark transform - // Eigen::Vector7d cam_pose_lmk = best_feature->getMeasurement(); - // Eigen::Quaterniond cam_q_lmk(cam_pose_lmk.segment<4>(3).data()); - // Eigen::Isometry3d cam_M_lmk = Eigen::Translation3d(cam_pose_lmk.head(3)) * cam_q_lmk; - - // // Get corresponding landmarks in origin/last landmark list - // Eigen::Isometry3d w_M_lmk; - // LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList(); - // // Iterate in reverse order because landmark detected in last are at the end of the list (while still landmarks to discovers) - // for (std::list<LandmarkBasePtr>::reverse_iterator it_lmk = lmk_list.rbegin(); it_lmk != lmk_list.rend(); ++it_lmk){ - // LandmarkObjectPtr lmk_ptr = std::dynamic_pointer_cast<LandmarkObject>(*it_lmk); - // // the map might contain other types of landmarks so check if the cast is valid - // if (lmk_ptr == nullptr){ - // continue; - // } - - // if (lmk_ptr->getTagId() == best_feature->getTagId()){ - // Eigen::Vector3d w_t_lmk = lmk_ptr->getP()->getState(); - // Eigen::Quaterniond w_q_lmk(lmk_ptr->getO()->getState().data()); - // w_M_lmk = Eigen::Translation<double,3>(w_t_lmk) * w_q_lmk; - // } - // } - - // // Compute last frame estimate - // Eigen::Isometry3d w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse(); - - // // Use the w_M_last to overide last key frame state estimation and keyframe estimation - // Eigen::Vector3d pos_last = w_M_last.translation(); - // Eigen::Quaterniond quat_last(w_M_last.linear()); - // getLast()->getFrame()->getP()->setState(pos_last); - // getLast()->getFrame()->getO()->setState(quat_last.coeffs()); - - // // if (!best_feature->getUserotation()){ - // // return; - // // } - // /////////////////// - // // Reestimate position of landmark new in Last - // /////////////////// - // for (auto it_feat = new_features_last_.begin(); it_feat != new_features_last_.end(); it_feat++){ - // FeatureObjectPtr new_feature_last = std::static_pointer_cast<FeatureObject>(*it_feat); - - // Eigen::Vector7d cam_pose_lmk = new_feature_last->getMeasurement(); - // Eigen::Quaterniond cam_q_lmk(cam_pose_lmk.segment<4>(3).data()); - // Eigen::Isometry3d cam_M_lmk_new = Eigen::Translation3d(cam_pose_lmk.head(3)) * cam_q_lmk; - // Eigen::Isometry3d w_M_lmk = w_M_last * last_M_cam * cam_M_lmk_new; - - // for (auto it_lmk = new_landmarks_.begin(); it_lmk != new_landmarks_.end(); ++it_lmk){ - // LandmarkObjectPtr lmk_ptr = std::dynamic_pointer_cast<LandmarkObject>(*it_lmk); - // if (lmk_ptr == nullptr){ - // continue; - // } - // if (lmk_ptr->getTagId() == new_feature_last->getTagId()){ - // Eigen::Vector3d pos_lmk_last = w_M_lmk.translation(); - // Eigen::Quaterniond quat_lmk_last(w_M_lmk.linear()); - // lmk_ptr->getP()->setState(pos_lmk_last); - // lmk_ptr->getO()->setState(quat_lmk_last.coeffs()); - // break; - // } - // } - // } -} - } // namespace wolf diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp index 6ee563efecfb1b0ffd520eece02b483f589cd2d8..d8dd187fe3f86212dbc90d0218f5b6819d336a84 100644 --- a/test/gtest_processor_tracker_landmark_object.cpp +++ b/test/gtest_processor_tracker_landmark_object.cpp @@ -82,7 +82,7 @@ class ProcessorTrackerLandmarkObject_class : public testing::Test{ // set prior VectorComposite x0("PO", {Vector3d(0,0,0), Quaterniond::Identity().coeffs()}); VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)}); - F1 = problem->setPriorFactor(x0, s0, 0.0, 0.1); + F1 = problem->setPriorFactor(x0, s0, 0.0); // minimal config for the processor to be operative C1 = CaptureBase::emplace<CapturePose>(F1, 1.0, sen, Vector7d(), Matrix6d());