From 6fb1018327d6c95ba0e0bf87ac251e7087c1864b Mon Sep 17 00:00:00 2001
From: artivis <deray.jeremie@gmail.com>
Date: Sun, 25 Mar 2018 14:56:00 +0200
Subject: [PATCH] bs
---
CMakeLists.txt | 34 ++++++++++-----------------
src/solver/solver_manager.cpp | 34 +++++++++++++++++++++++++++
src/test/gtest_IMU.cpp | 6 ++---
src/test/gtest_odom_2D.cpp | 3 ++-
src/test/serialization/CMakeLists.txt | 2 +-
5 files changed, 52 insertions(+), 27 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e2b1e20a7..180076923 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,28 +12,18 @@ SET(CMAKE_MACOSX_RPATH 1)
# The project name
PROJECT(wolf)
-
-#string(COMPARE EQUAL "${CMAKE_BINARY_DIR}" "" result)
-#IF(result)
-# SET(CMAKE_BINARY_DIR ${CMAKE_CURRENT_SOURCE_DIR})
-#ENDIF()
-#message(STATUS "Binary path : " ${CMAKE_BINARY_DIR})
-#
-#SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin)
-#SET(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib)
-#
-# We'll set the install prefix only is it's empty
-# which shouldn't be the case ...
-#string(COMPARE EQUAL "${CMAKE_INSTALL_PREFIX}" "" result)
-#IF(result)
- # This path is actually default on linux
-# SET(CMAKE_INSTALL_PREFIX /usr/local)
-#ENDIF()
-#message(STATUS "Installation path : " ${CMAKE_INSTALL_PREFIX})
-
-SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
-SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
-SET(CMAKE_INSTALL_PREFIX /usr/local)
+SET(CATKIN_BUILD TRUE)
+IF(CATKIN_TOPLEVEL OR CATKIN_BUILD_BINARY_PACKAGE OR CATKIN_SKIP_TESTING OR CATKIN_ENABLE_TESTING OR CATKIN_DEVEL_PREFIX)
+ SET(CATKIN_BUILD TRUE)
+ENDIF(CATKIN_TOPLEVEL OR CATKIN_BUILD_BINARY_PACKAGE OR CATKIN_SKIP_TESTING OR CATKIN_ENABLE_TESTING OR CATKIN_DEVEL_PREFIX)
+
+#message(FATAL_ERROR "CATKIN_BUILD " ${CATKIN_BUILD})
+
+IF (NOT CATKIN_BUILD)
+ #SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
+ #SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
+ #SET(CMAKE_INSTALL_PREFIX /usr/local)
+ENDIF (NOT CATKIN_BUILD)
IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "DEBUG")
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 29ca526d0..3d5211511 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -13,6 +13,29 @@ SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
void SolverManager::update()
{
+// while (!wolf_problem_->getConstraintNotificationList().empty())
+// {
+// switch (wolf_problem_->getConstraintNotificationList().front().notification_)
+// {
+// case Notification::ADD:
+// case Notification::UPDATE:
+// {
+// break;
+// }
+// case Notification::REMOVE:
+// {
+// removeConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
+
+// break;
+// }
+//// default:
+//// throw std::runtime_error("SolverManager::update:"
+//// " Constraint notification must be ADD or REMOVE.");
+// }
+
+// wolf_problem_->getConstraintNotificationList().pop_front();
+// }
+
StateBlockList& states = wolf_problem_->getNotifiedStateBlockList();
for (StateBlockPtr& state : states)
@@ -71,6 +94,17 @@ void SolverManager::update()
break;
}
+ case Notification::UPDATE:
+ {
+ WOLF_DEBUG("Called constraint update.");
+ break;
+ }
+ case Notification::REMOVE:
+ {
+ removeConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
+
+ break;
+ }
default:
throw std::runtime_error("SolverManager::update:"
" Constraint notification must be ADD or REMOVE.");
diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp
index 38dfbc223..6ed10f2d4 100644
--- a/src/test/gtest_IMU.cpp
+++ b/src/test/gtest_IMU.cpp
@@ -84,7 +84,7 @@ class Process_Constraint_IMU : public testing::Test
bool p1_fixed, q1_fixed, v1_fixed;
- virtual void SetUp( )
+ virtual void SetUp( ) override
{
string wolf_root = _WOLF_ROOT_DIR;
@@ -736,7 +736,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimat
assertAll();
}
-
+/*
TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated
{
@@ -1548,7 +1548,7 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)
ASSERT_MATRIX_APPROX(Trj_x_optim_prc.rightCols(1), x1_exact, 1e-6);
}
-
+*/
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp
index 007b3b834..71a7ccbaa 100644
--- a/src/test/gtest_odom_2D.cpp
+++ b/src/test/gtest_odom_2D.cpp
@@ -214,6 +214,8 @@ TEST(Odom2D, VoteForKfAndSolve)
ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+// return ;
+
// std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl;
// std::cout << "Initial covariance : " << std::endl << problem->getLastKeyFrameCovariance() << std::endl;
// std::cout << "Motion data : " << data.transpose() << std::endl;
@@ -439,7 +441,6 @@ TEST(Odom2D, KF_callback)
report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-
// check the split KF
ASSERT_POSE2D_APPROX(keyframe_1->getState() , integrated_pose_vector[m_split], 1e-6);
ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_1) , integrated_cov_vector [m_split], 1e-6); // FIXME test does not pass
diff --git a/src/test/serialization/CMakeLists.txt b/src/test/serialization/CMakeLists.txt
index 6902132bd..0c907aeb6 100644
--- a/src/test/serialization/CMakeLists.txt
+++ b/src/test/serialization/CMakeLists.txt
@@ -1,4 +1,4 @@
# cereal
IF(cereal_FOUND)
- add_subdirectory(cereal)
+ #add_subdirectory(cereal)
ENDIF(cereal_FOUND)
--
GitLab