Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions .gitmodules
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
[submodule "vcpkg"]
path = vcpkg
url = https://github.com/microsoft/vcpkg.git
55 changes: 30 additions & 25 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,48 +1,53 @@
cmake_minimum_required(VERSION 3.10)

set(CMAKE_TOOLCHAIN_FILE
"${CMAKE_CURRENT_SOURCE_DIR}/vcpkg/scripts/buildsystems/vcpkg.cmake"
CACHE STRING "Vcpkg toolchain file")

project(ob_gins)

set(CMAKE_CXX_STANDARD 17)

if (CMAKE_BUILD_TYPE MATCHES "Debug")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
else ()
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3")
endif ()
if(MSVC)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} /utf-8")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /utf-8")
add_compile_options(/permissive-)
else()
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)

file(GLOB_RECURSE SOURCE
src/ob_gins.cc
src/fileio/fileloader.cc
src/fileio/filesaver.cc
src/factors/pose_manifold.cc
src/preintegration/preintegration_base.cc
src/preintegration/preintegration_normal.cc
src/preintegration/preintegration_earth.cc
src/preintegration/preintegration_odo.cc
src/preintegration/preintegration_earth_odo.cc)
add_compile_definitions(M_PI=3.14159265358979323846)

file(
GLOB_RECURSE
SOURCE
src/ob_gins.cc
src/fileio/fileloader.cc
src/fileio/filesaver.cc
src/factors/pose_manifold.cc
src/preintegration/preintegration_base.cc
src/preintegration/preintegration_normal.cc
src/preintegration/preintegration_earth.cc
src/preintegration/preintegration_odo.cc
src/preintegration/preintegration_earth_odo.cc)

include_directories(${PROJECT_SOURCE_DIR})
add_executable(${PROJECT_NAME} ${SOURCE})

# Eigen3
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)

# yaml-cpp
find_package(yaml-cpp REQUIRED)
target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES})
target_link_libraries(${PROJECT_NAME} yaml-cpp::yaml-cpp)

# abseil
set(ABSL_PROPAGATE_CXX_STD true)
add_subdirectory(src/thirdparty/abseil-cpp abseil-cpp)
target_link_libraries(${PROJECT_NAME}
absl::strings
absl::str_format
absl::time)
target_link_libraries(${PROJECT_NAME} absl::strings absl::str_format absl::time)

# Ceres
find_package(Ceres REQUIRED)
target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES})
target_link_libraries(${PROJECT_NAME} Ceres::ceres)
20 changes: 10 additions & 10 deletions src/ob_gins.cc
Original file line number Diff line number Diff line change
Expand Up @@ -187,16 +187,16 @@ int main(int argc, char *argv[]) {

// 初始状态
// initialization
IntegrationState state_curr = {
.time = round(gnss.time),
.p = gnss.blh - Rotation::euler2quaternion(initatt) * antlever,
.q = Rotation::euler2quaternion(initatt),
.v = initvel,
.bg = initbg,
.ba = initba,
.sodo = 0.0,
.abv = {bodyangle[1], bodyangle[2]},
};
IntegrationState state_curr;
state_curr.time = round(gnss.time);
state_curr.p = gnss.blh - Rotation::euler2quaternion(initatt) * antlever;
state_curr.q = Rotation::euler2quaternion(initatt);
state_curr.v = initvel;
state_curr.bg = initbg;
state_curr.ba = initba;
state_curr.sodo = 0.0;
state_curr.abv = {bodyangle[1], bodyangle[2]};

std::cout << "Initilization at " << gnss.time << " s " << std::endl;

statelist[0] = state_curr;
Expand Down
27 changes: 12 additions & 15 deletions src/preintegration/preintegration_earth.cc
Original file line number Diff line number Diff line change
Expand Up @@ -184,21 +184,18 @@ IntegrationState PreintegrationEarth::stateFromData(const IntegrationStateData &

void PreintegrationEarth::constructState(const double *const *parameters, IntegrationState &state0,
IntegrationState &state1) {
state0 = IntegrationState{
.p = {parameters[0][0], parameters[0][1], parameters[0][2]},
.q = {parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]},
.v = {parameters[1][0], parameters[1][1], parameters[1][2]},
.bg = {parameters[1][3], parameters[1][4], parameters[1][5]},
.ba = {parameters[1][6], parameters[1][7], parameters[1][8]},
};

state1 = IntegrationState{
.p = {parameters[2][0], parameters[2][1], parameters[2][2]},
.q = {parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]},
.v = {parameters[3][0], parameters[3][1], parameters[3][2]},
.bg = {parameters[3][3], parameters[3][4], parameters[3][5]},
.ba = {parameters[3][6], parameters[3][7], parameters[3][8]},
};
state0 = IntegrationState();
state0.p = {parameters[0][0], parameters[0][1], parameters[0][2]};
state0.q = {parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]};
state0.v = {parameters[1][0], parameters[1][1], parameters[1][2]};
state0.bg = {parameters[1][3], parameters[1][4], parameters[1][5]};
state0.ba = {parameters[1][6], parameters[1][7], parameters[1][8]};
state1 = IntegrationState();
state1.p = {parameters[2][0], parameters[2][1], parameters[2][2]};
state1.q = {parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]};
state1.v = {parameters[3][0], parameters[3][1], parameters[3][2]};
state1.bg = {parameters[3][3], parameters[3][4], parameters[3][5]};
state1.ba = {parameters[3][6], parameters[3][7], parameters[3][8]};
}

void PreintegrationEarth::integrationProcess(unsigned long index) {
Expand Down
32 changes: 15 additions & 17 deletions src/preintegration/preintegration_earth_odo.cc
Original file line number Diff line number Diff line change
Expand Up @@ -208,23 +208,21 @@ IntegrationState PreintegrationEarthOdo::stateFromData(const IntegrationStateDat

void PreintegrationEarthOdo::constructState(const double *const *parameters, IntegrationState &state0,
IntegrationState &state1) {
state0 = IntegrationState{
.p = {parameters[0][0], parameters[0][1], parameters[0][2]},
.q = {parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]},
.v = {parameters[1][0], parameters[1][1], parameters[1][2]},
.bg = {parameters[1][3], parameters[1][4], parameters[1][5]},
.ba = {parameters[1][6], parameters[1][7], parameters[1][8]},
.sodo = parameters[1][9],
};

state1 = IntegrationState{
.p = {parameters[2][0], parameters[2][1], parameters[2][2]},
.q = {parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]},
.v = {parameters[3][0], parameters[3][1], parameters[3][2]},
.bg = {parameters[3][3], parameters[3][4], parameters[3][5]},
.ba = {parameters[3][6], parameters[3][7], parameters[3][8]},
.sodo = parameters[3][9],
};
state0 = IntegrationState();
state0.p = {parameters[0][0], parameters[0][1], parameters[0][2]};
state0.q = {parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]};
state0.v = {parameters[1][0], parameters[1][1], parameters[1][2]};
state0.bg = {parameters[1][3], parameters[1][4], parameters[1][5]};
state0.ba = {parameters[1][6], parameters[1][7], parameters[1][8]};
state0.sodo = parameters[1][9];

state1 = IntegrationState();
state1.p = {parameters[2][0], parameters[2][1], parameters[2][2]};
state1.q = {parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]};
state1.v = {parameters[3][0], parameters[3][1], parameters[3][2]};
state1.bg = {parameters[3][3], parameters[3][4], parameters[3][5]};
state1.ba = {parameters[3][6], parameters[3][7], parameters[3][8]};
state1.sodo = parameters[3][9];
}

void PreintegrationEarthOdo::integrationProcess(unsigned long index) {
Expand Down
27 changes: 12 additions & 15 deletions src/preintegration/preintegration_normal.cc
Original file line number Diff line number Diff line change
Expand Up @@ -163,21 +163,18 @@ IntegrationState PreintegrationNormal::stateFromData(const IntegrationStateData

void PreintegrationNormal::constructState(const double *const *parameters, IntegrationState &state0,
IntegrationState &state1) {
state0 = IntegrationState{
.p = {parameters[0][0], parameters[0][1], parameters[0][2]},
.q = {parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]},
.v = {parameters[1][0], parameters[1][1], parameters[1][2]},
.bg = {parameters[1][3], parameters[1][4], parameters[1][5]},
.ba = {parameters[1][6], parameters[1][7], parameters[1][8]},
};

state1 = IntegrationState{
.p = {parameters[2][0], parameters[2][1], parameters[2][2]},
.q = {parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]},
.v = {parameters[3][0], parameters[3][1], parameters[3][2]},
.bg = {parameters[3][3], parameters[3][4], parameters[3][5]},
.ba = {parameters[3][6], parameters[3][7], parameters[3][8]},
};
state0 = IntegrationState();
state0.p = {parameters[0][0], parameters[0][1], parameters[0][2]};
state0.q = {parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]};
state0.v = {parameters[1][0], parameters[1][1], parameters[1][2]};
state0.bg = {parameters[1][3], parameters[1][4], parameters[1][5]};
state0.ba = {parameters[1][6], parameters[1][7], parameters[1][8]};
state1 = IntegrationState();
state1.p = {parameters[2][0], parameters[2][1], parameters[2][2]};
state1.q = {parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]};
state1.v = {parameters[3][0], parameters[3][1], parameters[3][2]};
state1.bg = {parameters[3][3], parameters[3][4], parameters[3][5]};
state1.ba = {parameters[3][6], parameters[3][7], parameters[3][8]};
}

void PreintegrationNormal::integrationProcess(unsigned long index) {
Expand Down
34 changes: 16 additions & 18 deletions src/preintegration/preintegration_odo.cc
Original file line number Diff line number Diff line change
Expand Up @@ -184,23 +184,21 @@ IntegrationState PreintegrationOdo::stateFromData(const IntegrationStateData &da

void PreintegrationOdo::constructState(const double *const *parameters, IntegrationState &state0,
IntegrationState &state1) {
state0 = IntegrationState{
.p = {parameters[0][0], parameters[0][1], parameters[0][2]},
.q = {parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]},
.v = {parameters[1][0], parameters[1][1], parameters[1][2]},
.bg = {parameters[1][3], parameters[1][4], parameters[1][5]},
.ba = {parameters[1][6], parameters[1][7], parameters[1][8]},
.sodo = parameters[1][9],
};

state1 = IntegrationState{
.p = {parameters[2][0], parameters[2][1], parameters[2][2]},
.q = {parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]},
.v = {parameters[3][0], parameters[3][1], parameters[3][2]},
.bg = {parameters[3][3], parameters[3][4], parameters[3][5]},
.ba = {parameters[3][6], parameters[3][7], parameters[3][8]},
.sodo = parameters[3][9],
};
state0 = IntegrationState();
state0.p = {parameters[0][0], parameters[0][1], parameters[0][2]};
state0.q = {parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]};
state0.v = {parameters[1][0], parameters[1][1], parameters[1][2]};
state0.bg = {parameters[1][3], parameters[1][4], parameters[1][5]};
state0.ba = {parameters[1][6], parameters[1][7], parameters[1][8]};
state0.sodo = parameters[1][9];

state1 = IntegrationState();
state1.p = {parameters[2][0], parameters[2][1], parameters[2][2]};
state1.q = {parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]};
state1.v = {parameters[3][0], parameters[3][1], parameters[3][2]};
state1.bg = {parameters[3][3], parameters[3][4], parameters[3][5]};
state1.ba = {parameters[3][6], parameters[3][7], parameters[3][8]};
state1.sodo = parameters[3][9];
}

void PreintegrationOdo::integrationProcess(unsigned long index) {
Expand Down Expand Up @@ -331,4 +329,4 @@ void PreintegrationOdo::imuErrorJacobian(double *jacobian) {
jaco(4, 7) = 1.0 / IMU_ACC_BIAS_STD;
jaco(5, 8) = 1.0 / IMU_ACC_BIAS_STD;
jaco(6, 9) = 1.0 / ODO_SCALE_STD;
}
}
1 change: 1 addition & 0 deletions vcpkg
Submodule vcpkg added at 633afd
17 changes: 17 additions & 0 deletions vcpkg.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
{
"dependencies": [
{
"name": "ceres",
"features": ["eigensparse"]
},
"eigen3",
"yaml-cpp"
],
"builtin-baseline": "633afd0db78b420739f3817976a42ed59bd1530c",
"overrides": [
{
"name": "ceres",
"version": "2.1.0#5"
}
]
}