i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-12 06:20:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

android: fixed g2o seg fault (CACHE_SE3_OFFSET not registered)

Browse Source
This commit is contained in:
matlabbe
2022-01-02 23:30:55 -05:00
parent f3ace6c86a
commit b2f13c2772
6 changed files with 110 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/jni/RTABMapApp.cpp
+43
View File
@@ -83,6 +83,43 @@ static JavaVM *jvm;
static jobject RTABMapActivity = 0;
#endif
#ifdef __ANDROID__
#ifndef DISABLE_LOG
//ref: https://codelab.wordpress.com/2014/11/03/how-to-use-standard-output-streams-for-logging-in-android-apps/
static int pfd[2];
static pthread_t thr;
static void *thread_func(void*)
{
ssize_t rdsz;
char buf[128];
while((rdsz = read(pfd[0], buf, sizeof buf - 1)) > 0) {
if(buf[rdsz - 1] == '\n') --rdsz;
buf[rdsz] = 0; /* add null-terminator */
__android_log_write(ANDROID_LOG_DEBUG, LOG_TAG, buf);
}
return 0;
}
int start_logger()
{
/* make stdout line-buffered and stderr unbuffered */
setvbuf(stdout, 0, _IOLBF, 0);
setvbuf(stderr, 0, _IONBF, 0);
/* create the pipe and redirect stdout and stderr */
pipe(pfd);
dup2(pfd[1], 1);
dup2(pfd[1], 2);
/* spawn the logging thread */
if(pthread_create(&thr, 0, thread_func, 0) == -1)
return -1;
pthread_detach(thr);
return 0;
}
#endif
#endif
rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
{
rtabmap::ParametersMap parameters;
@@ -244,6 +281,12 @@ RTABMapApp::RTABMapApp() :
this->registerToEventsManager();
LOGI("RTABMapApp::RTABMapApp() end");
#ifdef __ANDROID__
#ifndef DISABLE_LOG
start_logger();
#endif
#endif
}
#ifndef __ANDROID__ // __APPLE__
app/android/src/com/introlab/rtabmap/RTABMapActivity.java
+1 -2
View File
@@ -1986,6 +1986,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
final int loopDetected = RTABMapLib.postProcessing(nativeApplication, -1);
runOnUiThread(new Runnable() {
public void run() {
updateState(State.STATE_IDLE);
if(mExportProgressDialog.isShowing())
{
mExportProgressDialog.dismiss();
@@ -2018,8 +2019,6 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
mProgressDialog.dismiss();
mToast.makeText(getActivity(), String.format("Optimization canceled"), mToast.LENGTH_LONG).show();
}
updateState(State.STATE_IDLE);
}
});
}
cmake_modules/FindG2O.cmake
+1
View File
@@ -1,4 +1,5 @@
#Pre-requisites: Look for csparse
MESSAGE(STATUS "RTAB-Map's cmake g2o find module used for convenience (for older g2o versions)")
FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h PATH_SUFFIXES suitesparse csparse EXTERNAL/suitesparse EXTERNAL/csparse g2o/EXTERNAL/suitesparse g2o/EXTERNAL/csparse
PATHS "C:\\Program Files\\g2o\\include\\EXTERNAL")
FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse g2o_ext_csparse
corelib/src/CMakeLists.txt
+13 -3
View File
@@ -376,15 +376,25 @@ IF(G2O_FOUND)
SET(LIBRARIES
${LIBRARIES}
g2o::core
g2o::solver_cholmod
g2o::solver_eigen
g2o::solver_pcg
g2o::solver_csparse
g2o::csparse_extension
g2o::types_slam2d
g2o::types_slam3d
g2o::types_sba
)
IF(TARGET g2o::solver_csparse)
SET(LIBRARIES
${LIBRARIES}
g2o::solver_csparse
g2o::csparse_extension
)
ENDIF(TARGET g2o::solver_csparse)
IF(TARGET g2o::solver_cholmod)
SET(LIBRARIES
${LIBRARIES}
g2o::solver_cholmod
)
ENDIF(TARGET g2o::solver_csparse)
ELSE()
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
corelib/src/optimizer/OptimizerG2O.cpp
+46 -25
View File
@@ -149,6 +149,16 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
UASSERT(pixelVariance_ > 0.0);
UASSERT(baseline_ >= 0.0);
// Issue on android, have to explicitly register this type when using fixed root prior below
if(!g2o::Factory::instance()->knowsTag("CACHE_SE3_OFFSET"))
{
#ifdef RTABMAP_G2O_CPP11
g2o::Factory::instance()->registerType("CACHE_SE3_OFFSET", g2o::make_unique<g2o::HyperGraphElementCreator<g2o::CacheSE3Offset> >());
#else
g2o::Factory::instance()->registerType("CACHE_SE3_OFFSET", new g2o::HyperGraphElementCreator<g2o::CacheSE3Offset>);
#endif
}
#ifdef RTABMAP_ORB_SLAM
if(solver_ != 3)
{
@@ -322,7 +332,9 @@ std::map<int, Transform> OptimizerG2O::optimize(
rootId = 0;
break;
}
else if(iter->second.type() == Link::kGravity)
else if(!isSlam2d() &&
gravitySigma() > 0 &&
iter->second.type() == Link::kGravity)
{
hasGravityConstraints = true;
if(priorsIgnored())
@@ -454,32 +466,41 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
vertex->setId(id);
UASSERT_MSG(optimizer.addVertex(vertex), uFormat("cannot insert vertex %d!?", iter->first).c_str());
if(!isSlam2d() && id == rootId && hasGravityConstraints)
}
}
// Setup root prior (fixed x,y,z,yaw)
if(!isSlam2d() && rootId !=0 && hasGravityConstraints)
{
g2o::VertexSE3* v1 = dynamic_cast<g2o::VertexSE3*>(optimizer.vertex(rootId));
if(v1)
{
g2o::EdgeSE3Prior * e = new g2o::EdgeSE3Prior();
e->setVertex(0, v1);
Eigen::Affine3d a = poses.at(rootId).toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
e->setMeasurement(pose);
e->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*10e6;
// pitch and roll not fixed
information(3,3) = information(4,4) = 1;
e->setInformation(information);
if (!optimizer.addEdge(e))
{
g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)vertex;
priorEdge->setVertex(0, v1);
Eigen::Affine3d a = iter->second.toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
priorEdge->setMeasurement(pose);
priorEdge->setParameterId(0, PARAM_OFFSET);
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*10e6;
// pitch and roll not fixed
information(3,3) = information(4,4) = 1;
priorEdge->setInformation(information);
if (priorEdge && !optimizer.addEdge(priorEdge))
{
delete priorEdge;
UERROR("Map: Failed adding fixed constraint of rootid %d, set as fixed instead", id);
v1->setFixed(true);
}
else
{
UDEBUG("Set %d fixed with prior (have gravity constraints)", id);
}
delete e;
UERROR("Map: Failed adding fixed constraint of rootid %d, set as fixed instead", rootId);
v1->setFixed(true);
}
else
{
UDEBUG("Set %d fixed with prior (have gravity constraints)", rootId);
}
}
else
{
UERROR("Map: Failed adding fixed constraint of rootid %d (not found in added vertices)", rootId);
}
}
docker/bionic/android/deps.bash
+6 -6
View File
@@ -16,7 +16,7 @@ wget -nv https://github.com/Kitware/CMake/releases/download/v3.17.0/cmake-3.17.0
echo "Downloading boost... (2/10)"
wget -nv https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz
echo "Downloading eigen... (3/10)"
wget -nv http://bitbucket.org/eigen/eigen/get/3.2.7.tar.gz
curl -L https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -o 3.3.9.tar.gz
echo "Downloading flann... (4/10)"
git clone -b 1.8.4 https://github.com/mariusmuja/flann.git
echo "Downloading gtsam... (5/10)"
@@ -31,7 +31,7 @@ echo "Downloading opencv_contrib... (9/10)"
git clone https://github.com/opencv/opencv_contrib.git
echo "Downloading opencv... (10/10)"
git clone https://github.com/opencv/opencv.git
-DG2O_USE_OPENGL=OFF
# Install directory for all dependencies
mkdir -p $prefix/arm64-v8a
@@ -57,15 +57,15 @@ rm -r boost_1_59_0.tar.gz boost_1_59_0
# eigen
echo "Install eigen..."
tar -xzf 3.2.7.tar.gz
cd eigen-eigen-b30b87236a1b
tar -xzf 3.3.9.tar.gz
cd eigen-3.3.9
mkdir build
cd build
$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" ..
make $cpus
make install
cd $pwd
rm -r 3.2.7.tar.gz eigen-eigen-b30b87236a1b
rm -r 3.3.9.tar.gz eigen-3.3.9
# FLANN
echo "Install flann..."
@@ -101,7 +101,7 @@ cd g2o
git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
mkdir build
cd build
$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF ..
$pwd/cmake-3.17.0-Linux-x86_64/bin/cmake -DCMAKE_TOOLCHAIN_FILE=$ANDROID_NDK/build/cmake/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DANDROID_NDK=$ANDROID_NDK -DANDROID_NATIVE_API_LEVEL=$ANDROID_NATIVE_API_LEVEL -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a -DCMAKE_FIND_ROOT_PATH="$prefix/arm64-v8a/bin;$prefix/arm64-v8a;$prefix/arm64-v8a/share" -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF ..
make $cpus
make install
cd $pwd