mit-spark / kimera-rpgo Goto Github PK

Hi, thanks for sharing this very good project! Currently this repository is missing a license file, it would be good to add one to make it safer to copy or redistribute the code.

Description:
When I execute make,I encountered a bug.
please help me,thinks.

The terminal output is as follows:
`Scanning dependencies of target KimeraRPGO
[ 6%] Building CXX object CMakeFiles/KimeraRPGO.dir/src/GenericSolver.cpp.o
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp: In member function ‘void KimeraRPGO::GenericSolver::removePriorsWithPrefix(const char&)’:
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:138:77: error: no matching function for call to ‘dynamic_pointer_cast<gtsam::PriorFactorgtsam::Pose3 >(std::shared_ptrgtsam::NonlinearFactor&)’
if (boost::dynamic_pointer_cast<gtsam::PriorFactorgtsam::Pose3>(factor)) {
^

In file included from /usr/local/include/boost/shared_ptr.hpp:17,
from /usr/local/include/boost/format/alt_sstream.hpp:21,
from /usr/local/include/boost/format/internals.hpp:24,
from /usr/local/include/boost/format.hpp:38,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/Logger.h:8,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/GenericSolver.h:16,
from /home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:9:
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:898:42: note: candidate: ‘template<class T, class U> boost::shared_ptr boost::dynamic_pointer_cast(const boost::shared_ptr&)’
template<class T, class U> shared_ptr dynamic_pointer_cast( shared_ptr const & r ) BOOST_SP_NOEXCEPT
^~~~~~~~~~~~~~~~~~~~
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:898:42: note: template argument deduction/substitution failed:
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:138:77: note: ‘std::shared_ptrgtsam::NonlinearFactor’ is not derived from ‘const boost::shared_ptr’
if (boost::dynamic_pointer_cast<gtsam::PriorFactorgtsam::Pose3>(factor)) {
^

In file included from /usr/local/include/boost/shared_ptr.hpp:17,
from /usr/local/include/boost/format/alt_sstream.hpp:21,
from /usr/local/include/boost/format/internals.hpp:24,
from /usr/local/include/boost/format.hpp:38,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/Logger.h:8,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/GenericSolver.h:16,
from /home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:9:
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:940:42: note: candidate: ‘template<class T, class U> boost::shared_ptr boost::dynamic_pointer_cast(boost::shared_ptr&&)’
template<class T, class U> shared_ptr dynamic_pointer_cast( shared_ptr && r ) BOOST_SP_NOEXCEPT
^~~~~~~~~~~~~~~~~~~~
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:940:42: note: template argument deduction/substitution failed:
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:138:77: note: ‘std::shared_ptrgtsam::NonlinearFactor’ is not derived from ‘boost::shared_ptr’
if (boost::dynamic_pointer_cast<gtsam::PriorFactorgtsam::Pose3>(factor)) {
^

/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:141:21: error: no matching function for call to ‘dynamic_pointer_cast<gtsam::PriorFactorgtsam::Pose3 >(std::shared_ptrgtsam::NonlinearFactor&)’
factor);
^
In file included from /usr/local/include/boost/shared_ptr.hpp:17,
from /usr/local/include/boost/format/alt_sstream.hpp:21,
from /usr/local/include/boost/format/internals.hpp:24,
from /usr/local/include/boost/format.hpp:38,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/Logger.h:8,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/GenericSolver.h:16,
from /home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:9:
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:898:42: note: candidate: ‘template<class T, class U> boost::shared_ptr boost::dynamic_pointer_cast(const boost::shared_ptr&)’
template<class T, class U> shared_ptr dynamic_pointer_cast( shared_ptr const & r ) BOOST_SP_NOEXCEPT
^~~~~~~~~~~~~~~~~~~~
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:898:42: note: template argument deduction/substitution failed:
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:141:21: note: ‘std::shared_ptrgtsam::NonlinearFactor’ is not derived from ‘const boost::shared_ptr’
factor);
^
In file included from /usr/local/include/boost/shared_ptr.hpp:17,
from /usr/local/include/boost/format/alt_sstream.hpp:21,
from /usr/local/include/boost/format/internals.hpp:24,
from /usr/local/include/boost/format.hpp:38,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/Logger.h:8,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/GenericSolver.h:16,
from /home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:9:
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:940:42: note: candidate: ‘template<class T, class U> boost::shared_ptr boost::dynamic_pointer_cast(boost::shared_ptr&&)’
template<class T, class U> shared_ptr dynamic_pointer_cast( shared_ptr && r ) BOOST_SP_NOEXCEPT
^~~~~~~~~~~~~~~~~~~~
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:940:42: note: template argument deduction/substitution failed:
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:141:21: note: ‘std::shared_ptrgtsam::NonlinearFactor’ is not derived from ‘boost::shared_ptr’
factor);
^
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:145:26: error: no matching function for call to ‘dynamic_pointer_cast<gtsam::PriorFactorgtsam::Pose2 >(std::shared_ptrgtsam::NonlinearFactor&)’
factor)) {
^
In file included from /usr/local/include/boost/shared_ptr.hpp:17,
from /usr/local/include/boost/format/alt_sstream.hpp:21,
from /usr/local/include/boost/format/internals.hpp:24,
from /usr/local/include/boost/format.hpp:38,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/Logger.h:8,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/GenericSolver.h:16,
from /home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:9:
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:898:42: note: candidate: ‘template<class T, class U> boost::shared_ptr boost::dynamic_pointer_cast(const boost::shared_ptr&)’
template<class T, class U> shared_ptr dynamic_pointer_cast( shared_ptr const & r ) BOOST_SP_NOEXCEPT
^~~~~~~~~~~~~~~~~~~~
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:898:42: note: template argument deduction/substitution failed:
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:145:26: note: ‘std::shared_ptrgtsam::NonlinearFactor’ is not derived from ‘const boost::shared_ptr’
factor)) {
^
In file included from /usr/local/include/boost/shared_ptr.hpp:17,
from /usr/local/include/boost/format/alt_sstream.hpp:21,
from /usr/local/include/boost/format/internals.hpp:24,
from /usr/local/include/boost/format.hpp:38,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/Logger.h:8,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/GenericSolver.h:16,
from /home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:9:
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:940:42: note: candidate: ‘template<class T, class U> boost::shared_ptr boost::dynamic_pointer_cast(boost::shared_ptr&&)’
template<class T, class U> shared_ptr dynamic_pointer_cast( shared_ptr && r ) BOOST_SP_NOEXCEPT
^~~~~~~~~~~~~~~~~~~~
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:940:42: note: template argument deduction/substitution failed:
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:145:26: note: ‘std::shared_ptrgtsam::NonlinearFactor’ is not derived from ‘boost::shared_ptr’
factor)) {
^
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:148:21: error: no matching function for call to ‘dynamic_pointer_cast<gtsam::PriorFactorgtsam::Pose2 >(std::shared_ptrgtsam::NonlinearFactor&)’
factor);
^
In file included from /usr/local/include/boost/shared_ptr.hpp:17,
from /usr/local/include/boost/format/alt_sstream.hpp:21,
from /usr/local/include/boost/format/internals.hpp:24,
from /usr/local/include/boost/format.hpp:38,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/Logger.h:8,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/GenericSolver.h:16,
from /home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:9:
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:898:42: note: candidate: ‘template<class T, class U> boost::shared_ptr boost::dynamic_pointer_cast(const boost::shared_ptr&)’
template<class T, class U> shared_ptr dynamic_pointer_cast( shared_ptr const & r ) BOOST_SP_NOEXCEPT
^~~~~~~~~~~~~~~~~~~~
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:898:42: note: template argument deduction/substitution failed:
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:148:21: note: ‘std::shared_ptrgtsam::NonlinearFactor’ is not derived from ‘const boost::shared_ptr’
factor);
^
In file included from /usr/local/include/boost/shared_ptr.hpp:17,
from /usr/local/include/boost/format/alt_sstream.hpp:21,
from /usr/local/include/boost/format/internals.hpp:24,
from /usr/local/include/boost/format.hpp:38,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/Logger.h:8,
from /home/leng/Documents/Kimera-RPGO/include/KimeraRPGO/GenericSolver.h:16,
from /home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:9:
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:940:42: note: candidate: ‘template<class T, class U> boost::shared_ptr boost::dynamic_pointer_cast(boost::shared_ptr&&)’
template<class T, class U> shared_ptr dynamic_pointer_cast( shared_ptr && r ) BOOST_SP_NOEXCEPT
^~~~~~~~~~~~~~~~~~~~
/usr/local/include/boost/smart_ptr/shared_ptr.hpp:940:42: note: template argument deduction/substitution failed:
/home/leng/Documents/Kimera-RPGO/src/GenericSolver.cpp:148:21: note: ‘std::shared_ptrgtsam::NonlinearFactor’ is not derived from ‘boost::shared_ptr’
factor);
^
CMakeFiles/KimeraRPGO.dir/build.make:62: recipe for target 'CMakeFiles/KimeraRPGO.dir/src/GenericSolver.cpp.o' failed
make[2]: *** [CMakeFiles/KimeraRPGO.dir/src/GenericSolver.cpp.o] Error 1
CMakeFiles/Makefile2:153: recipe for target 'CMakeFiles/KimeraRPGO.dir/all' failed
make[1]: *** [CMakeFiles/KimeraRPGO.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
`
Please give also the following information:

Kimera-RPGO branch, tag or commit used: master
GTSAM version used: 9902ccc0a4f62123e91f057babe3612a95c15c20 (tag: 4.2a8)
Operating system and version (e.g. Ubuntu 16.04 or Windows 10): ubuntu 18.04
Did you change the source code? (yes / no): no,

I am trying to build in Ubuntu 18 and I obtain the following error:

/usr/include/boost/smart_ptr/shared_ptr.hpp:898:42: note: template argument deduction/substitution failed:
In file included from /home/javier/kimera_ws/src/Kimera-RPGO/src/RobustSolver.cpp:22:0:
/home/javier/kimera_ws/src/Kimera-RPGO/include/KimeraRPGO/outlier/Pcm.h:394:68: note: ‘std::shared_ptrgtsam::NonlinearFactor’ is not derived from ‘const boost::shared_ptr’
*boost::dynamic_pointer_cast<gtsam::PriorFactor>(factor);
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~
In file included from /usr/include/boost/shared_ptr.hpp:17:0,
from /usr/include/boost/date_time/gregorian/greg_month.hpp:14,
from /usr/include/boost/date_time/gregorian/greg_ymd.hpp:16,
from /usr/include/boost/date_time/gregorian/greg_calendar.hpp:17,
from /usr/include/boost/date_time/gregorian/gregorian_types.hpp:19,
from /usr/include/boost/date_time/posix_time/posix_time_config.hpp:18,
from /usr/include/boost/date_time/posix_time/posix_time_system.hpp:13,
from /usr/include/boost/date_time/posix_time/ptime.hpp:12,
from /usr/include/boost/date_time/posix_time/posix_time.hpp:15,
from /home/javier/kimera_ws/devel/include/gtsam/nonlinear/LevenbergMarquardtOptimizer.h:26,
from /home/javier/kimera_ws/devel/include/gtsam/nonlinear/GncParams.h:29,
from /home/javier/kimera_ws/devel/include/gtsam/nonlinear/GncOptimizer.h:29,
from /home/javier/kimera_ws/src/Kimera-RPGO/include/KimeraRPGO/RobustSolver.h:17,
from /home/javier/kimera_ws/src/Kimera-RPGO/src/RobustSolver.cpp:6:
/usr/include/boost/smart_ptr/shared_ptr.hpp:940:42: note: candidate: template<class T, class U> boost::shared_ptr boost::dynamic_pointer_cast(boost::shared_ptr&&)
template<class T, class U> shared_ptr dynamic_pointer_cast( shared_ptr && r ) BOOST_SP_NOEXCEPT
^~~~~~~~~~~~~~~~~~~~
/usr/include/boost/smart_ptr/shared_ptr.hpp:940:42: note: template argument deduction/substitution failed:
In file included from /home/javier/kimera_ws/src/Kimera-RPGO/src/RobustSolver.cpp:22:0:
/home/javier/kimera_ws/src/Kimera-RPGO/include/KimeraRPGO/outlier/Pcm.h:394:68: note: ‘std::shared_ptrgtsam::NonlinearFactor’ is not derived from ‘boost::shared_ptr’
*boost::dynamic_pointer_cast<gtsam::PriorFactor>(factor);
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~
make[2]: *** [CMakeFiles/KimeraRPGO.dir/src/RobustSolver.cpp.o] Error 1
make[1]: *** [CMakeFiles/KimeraRPGO.dir/all] Error 2

/home/tonirv/Code/RobustPGO/KimeraRPGO/utils/geometry_utils.h:224:28: error: ‘const class gtsam::BetweenFactor<gtsam::Pose3>’ has no member named ‘get_noiseModel’; did you mean ‘noiseModel’?
             between_factor.get_noiseModel())

Had to enable GTSAM_ALLOW_DEPRECATED_SINCE_V4 to fix this issue.
Would be great to move completely to V4 though...

Compilation of gtsam was successful but running into errors while compiling RPGO module.

[ 8%] Building CXX object CMakeFiles/KimeraRPGO.dir/src/RobustSolver.cpp.o
In file included from /home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:31:0,
from /home/kp/workspace/Kimera-RPGO/src/RobustSolver.cpp:19:
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h: In member function ‘double KimeraRPGO::PoseWithCovariance::mahalanobis_norm() const’:
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:179:61: error: ‘inverse’ is not a member of ‘gtsam’
return std::sqrt(log.tail(t_dim).transpose() * gtsam::inverse(cov_block) *
^~~~~~~
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:179:61: note: suggested alternative:
In file included from /usr/local/include/gtsam/3rdparty/Eigen/Eigen/Core:533:0,
from /usr/local/include/gtsam/3rdparty/Eigen/Eigen/Dense:1,
from /usr/local/include/gtsam/base/OptionalJacobian.h:22,
from /usr/local/include/gtsam/base/Matrix.h:27,
from /usr/local/include/gtsam/base/Manifold.h:22,
from /usr/local/include/gtsam/base/Lie.h:25,
from /usr/local/include/gtsam/base/VectorSpace.h:11,
from /usr/local/include/gtsam/geometry/Point2.h:20,
from /usr/local/include/gtsam/nonlinear/NonlinearFactorGraph.h:24,
from /home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/RobustSolver.h:11,
from /home/kp/workspace/Kimera-RPGO/src/RobustSolver.cpp:6:
/usr/local/include/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h:59:3: note: ‘Eigen::inverse’
EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(inverse,scalar_inverse_op,inverse,\sa ArrayBase::inverse)
^
In file included from /home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:31:0,
from /home/kp/workspace/Kimera-RPGO/src/RobustSolver.cpp:19:
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:183:47: error: ‘inverse’ is not a member of ‘gtsam’
return std::sqrt(log.transpose() * gtsam::inverse(covariance_matrix) * log);
^~~~~~~
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:183:47: note: suggested alternative:
In file included from /usr/local/include/gtsam/3rdparty/Eigen/Eigen/Core:533:0,
from /usr/local/include/gtsam/3rdparty/Eigen/Eigen/Dense:1,
from /usr/local/include/gtsam/base/OptionalJacobian.h:22,
from /usr/local/include/gtsam/base/Matrix.h:27,
from /usr/local/include/gtsam/base/Manifold.h:22,
from /usr/local/include/gtsam/base/Lie.h:25,
from /usr/local/include/gtsam/base/VectorSpace.h:11,
from /usr/local/include/gtsam/geometry/Point2.h:20,
from /usr/local/include/gtsam/nonlinear/NonlinearFactorGraph.h:24,
from /home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/RobustSolver.h:11,
from /home/kp/workspace/Kimera-RPGO/src/RobustSolver.cpp:6:
/usr/local/include/gtsam/3rdparty/Eigen/Eigen/src/Core/GlobalFunctions.h:59:3: note: ‘Eigen::inverse’
EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(inverse,scalar_inverse_op,inverse,\sa ArrayBase::inverse)
^
In file included from /home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:31:0,
from /home/kp/workspace/Kimera-RPGO/src/RobustSolver.cpp:19:
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h: In instantiation of ‘KimeraRPGO::PoseWithNode::PoseWithNode(const gtsam::BetweenFactor&) [with T = gtsam::Pose3]’:
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:374:14: required from ‘void KimeraRPGO::Pcm<poseT, T>::updateOdom(const shared_ptr&, const gtsam::Values&) [with poseT = gtsam::Pose3; T = KimeraRPGO::PoseWithNode; gtsam::NonlinearFactor::shared_ptr = boost::shared_ptrgtsam::NonlinearFactor]’
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:166:21: required from ‘bool KimeraRPGO::Pcm<poseT, T>::removeOutliers(const gtsam::NonlinearFactorGraph&, const gtsam::Values&, gtsam::NonlinearFactorGraph*, gtsam::Values*) [with poseT = gtsam::Pose3; T = KimeraRPGO::PoseWithNode]’
/home/kp/workspace/Kimera-RPGO/src/RobustSolver.cpp:151:1: required from here
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:223:28: error: ‘const class gtsam::BetweenFactorgtsam::Pose3’ has no member named ‘get_noiseModel’; did you mean ‘noiseModel’?
between_factor.get_noiseModel())
~~~~~~~~~~~~~~~^~~~~~~~~~~~~~
noiseModel
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:227:15: warning: unused variable ‘dim’ [-Wunused-variable]
const int dim = getDim();
^~~
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:229:15: warning: unused variable ‘t_dim’ [-Wunused-variable]
const int t_dim = getTranslationDim();
^~~~~
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h: In instantiation of ‘KimeraRPGO::PoseWithNode::PoseWithNode(const gtsam::BetweenFactor&) [with T = gtsam::Pose2]’:
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:374:14: required from ‘void KimeraRPGO::Pcm<poseT, T>::updateOdom(const shared_ptr&, const gtsam::Values&) [with poseT = gtsam::Pose2; T = KimeraRPGO::PoseWithNode; gtsam::NonlinearFactor::shared_ptr = boost::shared_ptrgtsam::NonlinearFactor]’
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:166:21: required from ‘bool KimeraRPGO::Pcm<poseT, T>::removeOutliers(const gtsam::NonlinearFactorGraph&, const gtsam::Values&, gtsam::NonlinearFactorGraph*, gtsam::Values*) [with poseT = gtsam::Pose2; T = KimeraRPGO::PoseWithNode]’
/home/kp/workspace/Kimera-RPGO/src/RobustSolver.cpp:151:1: required from here
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:223:28: error: ‘const class gtsam::BetweenFactorgtsam::Pose2’ has no member named ‘get_noiseModel’; did you mean ‘noiseModel’?
between_factor.get_noiseModel())
~~~~~~~~~~~~~~~^~~~~~~~~~~~~~
noiseModel
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:227:15: warning: unused variable ‘dim’ [-Wunused-variable]
const int dim = getDim();
^~~
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:229:15: warning: unused variable ‘t_dim’ [-Wunused-variable]
const int t_dim = getTranslationDim();
^~~~~
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h: In instantiation of ‘KimeraRPGO::PoseWithCovariance::PoseWithCovariance(const gtsam::BetweenFactor&) [with T = gtsam::Pose3]’:
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:374:14: required from ‘void KimeraRPGO::Pcm<poseT, T>::updateOdom(const shared_ptr&, const gtsam::Values&) [with poseT = gtsam::Pose3; T = KimeraRPGO::PoseWithCovariance; gtsam::NonlinearFactor::shared_ptr = boost::shared_ptrgtsam::NonlinearFactor]’
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:166:21: required from ‘bool KimeraRPGO::Pcm<poseT, T>::removeOutliers(const gtsam::NonlinearFactorGraph&, const gtsam::Values&, gtsam::NonlinearFactorGraph*, gtsam::Values*) [with poseT = gtsam::Pose3; T = KimeraRPGO::PoseWithCovariance]’
/home/kp/workspace/Kimera-RPGO/src/RobustSolver.cpp:151:1: required from here
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:93:28: error: ‘const class gtsam::BetweenFactorgtsam::Pose3’ has no member named ‘get_noiseModel’; did you mean ‘noiseModel’?
between_factor.get_noiseModel())
~~~~~~~~~~~~~~~^~~~~~~~~~~~~~
noiseModel
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h: In instantiation of ‘KimeraRPGO::PoseWithCovariance::PoseWithCovariance(const gtsam::BetweenFactor&) [with T = gtsam::Pose2]’:
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:374:14: required from ‘void KimeraRPGO::Pcm<poseT, T>::updateOdom(const shared_ptr&, const gtsam::Values&) [with poseT = gtsam::Pose2; T = KimeraRPGO::PoseWithCovariance; gtsam::NonlinearFactor::shared_ptr = boost::shared_ptrgtsam::NonlinearFactor]’
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/outlier/pcm.h:166:21: required from ‘bool KimeraRPGO::Pcm<poseT, T>::removeOutliers(const gtsam::NonlinearFactorGraph&, const gtsam::Values&, gtsam::NonlinearFactorGraph*, gtsam::Values*) [with poseT = gtsam::Pose2; T = KimeraRPGO::PoseWithCovariance]’
/home/kp/workspace/Kimera-RPGO/src/RobustSolver.cpp:151:1: required from here
/home/kp/workspace/Kimera-RPGO/include/KimeraRPGO/utils/geometry_utils.h:93:28: error: ‘const class gtsam::BetweenFactorgtsam::Pose2’ has no member named ‘get_noiseModel’; did you mean ‘noiseModel’?
between_factor.get_noiseModel())
~~~~~~~~~~~~~~~^~~~~~~~~~~~~~
noiseModel
CMakeFiles/KimeraRPGO.dir/build.make:86: recipe for target 'CMakeFiles/KimeraRPGO.dir/src/RobustSolver.cpp.o' failed
make[2]: *** [CMakeFiles/KimeraRPGO.dir/src/RobustSolver.cpp.o] Error 1
CMakeFiles/Makefile2:104: recipe for target 'CMakeFiles/KimeraRPGO.dir/all' failed
make[1]: *** [CMakeFiles/KimeraRPGO.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2

I'm compiling against GTSAM 4.0.3 and get the following error:

/home/pc/repos/vlu_pipeline/core/kimera-rpgo/src/RobustSolver.cpp:16:10: fatal error: gtsam/nonlinear/GncOptimizer.h: No such file or directory
   16 | #include <gtsam/nonlinear/GncOptimizer.h>                                                        
      |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~     

From what I can see, this file was only recently introduced in this commit:

ben@pc:~/repos/vlu_pipeline/support/gtsam$ git log --diff-filter=A -- gtsam/nonlinear/GncOptimizer.h
commit fcf2d316848cb54b50513af27f7a1f00501b119b
Author: lcarlone <[email protected]>
Date:   Sat Dec 5 13:47:40 2020 -0500

    moved class to .h

Which is newer than even the latest 4.1rc of GTSAM:

ben@pc:~/repos/vlu_pipeline/support/gtsam$ git log -n1 4.1rc 
commit 4c0e87be78053cbe26af4e18a4edea292d0bfb77 (tag: 4.1rc, origin/release/4.1.0)
Merge: 02cd45d4b b74c053c6
Author: Fan Jiang <[email protected]>
Date:   Sun Aug 23 22:52:04 2020 -0400

    Merge pull request #493 from borglab/feature/fix_foreach
    
    Make sure our readme is proper restructuredText

GTSAM uses a develop branch as the default which changes very frequently. Until there's a version that includes the required header, can you at least suggest a minimum required commit hash (like you've done for Kimera VIO: https://github.com/MIT-SPARK/Kimera-VIO/blob/master/docs/kimera_vio_install.md#gtsam-source-install)

Thanks

Hello,
I want to use this amazing RPGO in my project to estimate the pose of my robot. But I'm New in GTSAM and RPGO. Therefore, I'm struggling to install and use this Kimera-RPGO.
May anyone guide me here?
Thank you.

Hard to reproduce. Sometime I get this when I run make check after building. This usually happens when GTSAM is built with march-native on. But it's also inconsistent as sometimes it's fine and sometimes not.

[ 30%] Built target KimeraRPGO
[ 38%] Built target testLandmark
[ 46%] Built target testLoadGraph
[ 53%] Built target testPcmSimple
[ 61%] Built target testPcm
[ 69%] Built target testPoseWithNode
[ 76%] Built target testTrajectory
[ 84%] Built target testMultiRobot
[ 92%] Built target testPoseWithCovariance
[100%] Built target testPcmDoOptimize
Test project /home/yunchang/Kimera-RPGO/build
    Start 1: testLandmark
1/9 Test #1: testLandmark .....................***Exception: Child aborted  0.11 sec
double free or corruption (out)

    Start 2: testLoadGraph
2/9 Test #2: testLoadGraph ....................***Exception: Child aborted  0.09 sec
double free or corruption (out)

    Start 3: testMultiRobot
3/9 Test #3: testMultiRobot ...................***Exception: Child aborted  0.10 sec
double free or corruption (out)

    Start 4: testPcm
4/9 Test #4: testPcm ..........................***Exception: Child aborted  0.10 sec
double free or corruption (out)

    Start 5: testPcmDoOptimize
5/9 Test #5: testPcmDoOptimize ................***Exception: Child aborted  0.09 sec
double free or corruption (out)

    Start 6: testPcmSimple
6/9 Test #6: testPcmSimple ....................***Exception: Child aborted  0.09 sec
double free or corruption (out)

    Start 7: testPoseWithCovariance
7/9 Test #7: testPoseWithCovariance ...........***Exception: Child aborted  0.09 sec
double free or corruption (out)

    Start 8: testPoseWithNode
8/9 Test #8: testPoseWithNode .................   Passed    0.01 sec
    Start 9: testTrajectory
9/9 Test #9: testTrajectory ...................***Exception: Child aborted  0.10 sec

When using RobustPGO in Spark VIO I fail to compile with the following errors:

error: ‘isSpecialSymbol’ was not declared in this scope
         if (isSpecialSymbol(symb.chr())) {

error: ‘addAndCheckIfOptimize’ was not declared in this scope
               addAndCheckIfOptimize(new_factors, new_values);

and similar in other locations around the RobustSolver.cpp module. These don't pop up when I make and then ctest the RobustPGO project, so it might be worth it to try to replicate in a test or to at least figure out why the package compiles without error from the RobustPGO repo.

These failures are all inside of RobustSolver::update_batch(gtsam::NonlinearFactorGraph, const gtsam::Values&, const gtsam::Key&)

These members are protected members of the GenericSolver class, of which RobustSolver is a derived class. I don't see any reason why they shouldn't be accessible to the RobustSolver class.

Worth noting that I'm currently failing testLandmark, testLoadGraph, and testMultiRobot. I'm using feature/multirobot after the CMakeLists fix merge from fix/cmake_issues.

We should change these methods of GenericSolver.h:

gtsam::Values calculateEstimate() { return values_; }
gtsam::Values calculateBestEstimate() { return values_; }
gtsam::Values getLinearizationPoint() { return values_; }
gtsam::NonlinearFactorGraph getFactorsUnsafe(){ return nfg_; }

to this:

inline const gtsam::Values& calculateEstimate() const { return values_; }
inline const gtsam::Values& calculateBestEstimate() const { return values_;}
etc...

btw, calculateEstimate() and calculateBestEstimate() are doing the same thing. Should they be?

Hi,
Thanks for the project, but I think You need "shared export = dll_export and dll_import" for Solver classes to work with windows.

hi,
the enum class call doesn't work in clang

Thanks

Hi, thanks for the project!

In the code is see specializations for what looks like SE2 and SE3 pose graphs, but can this also work for Sim3?

When running VIO with loop closure detector, sometimes when a loop closure is detected there is a segfault during RobustPGO's output. Specifically, I add loop closures in the following way:

nfg.add(gtsam::BetweenFactor<gtsam::Pose3>(
    factor.ref_key_, factor.cur_key_, factor.ref_Pose_cur_, factor.noise_));

pgo_->update(nfg);

Then, when loops are detected I want to return the full trajectory. First I want a specific element from the values of the pgo so that I can compute an error transform. This is done like so:

gtsam::Pose3 w_Pose_Bkf_optimal =
      pgo_->calculateBestEstimate().at<gtsam::Pose3>(
          W_Pose_Bkf_estimates_.size() - 1);

W_Pose_Bkf_estimates_ is guaranteed to be of same size as pgo_'s values.

I get this error (in gdb):

Thread 1 "lcd_ros_node" received signal SIGSEGV, Segmentation fault.
0x00007ffff5f63610 in vtable for gtsam::BetweenFactor<gtsam::Pose3> ()
   from /home/marcus/code/VIO/build/libSparkVio.so
(gdb) backtrace
#0  0x00007ffff5f63610 in vtable for gtsam::BetweenFactor<gtsam::Pose3> ()
    at /home/marcus/code/VIO/build/libSparkVio.so
#1  0x00007ffff565d30d in gtsam::Values::tryInsert(unsigned long, gtsam::Value const&) ()
    at /usr/local/lib/libgtsam.so.4
#2  0x00007ffff565d64d in gtsam::Values::insert(unsigned long, gtsam::Value const&) ()
    at /usr/local/lib/libgtsam.so.4
#3  0x00007ffff565d811 in gtsam::Values::insert(gtsam::Values const&) () at /usr/local/lib/libgtsam.so.4
#4  0x00007ffff565da5d in gtsam::Values::Values(gtsam::Values const&) () at /usr/local/lib/libgtsam.so.4
#5  0x00007ffff5ade609 in GenericSolver::calculateBestEstimate() (this=<optimized out>)
    at /usr/local/include/RobustPGO/GenericSolver.h:46
#6  0x00007ffff5ade609 in VIO::LoopClosureDetector::getWPoseMap() const (this=0x5555561f9290)
    at /home/marcus/code/VIO/src/LoopClosureDetector.cpp:773
#7  0x00007ffff5ade609 in VIO::LoopClosureDetector::spinOnce(std::shared_ptr<VIO::LoopClosureDetectorInputPayload> const&) (this=0x5555561f9290, input=std::shared_ptr<VIO::LoopClosureDetectorInputPayload> (use count 1, weak count 0) = {...}) at /home/marcus/code/VIO/src/LoopClosureDetector.cpp:180
#8  0x000055555562dc59 in VIO::LCD_ros::spin() ()
#9  0x000055555561d5e1 in main ()

So during calculateBestEstimate there is an issue. But this simply returns the class gtsam::Values values member, so there shouldn't be anything wrong there. The backtrace suggests it is a problem in the gtsam library. @yunzc have you seen this before?

Hello author,
thank your work for implementation of PCM. I am confused about loading pose graphs. In your testLoadGraph.cpp:

you load 2 graphs at the same time. The Key used for loop closure are a1,b1. However, the keys in robot_a.g2o and robot_b.g2o are numbers:

I have just installed Kimera-RPGO on Ubuntu 20.04 LTS (WSL on Windows 10) and I'm trying to compile an example (Examples/RpgoReadG2o.cpp). However, the compilation doesn't go through due to the following error.

In file included from /usr/local/include/gtsam/base/Matrix.h:27,
from /usr/local/include/gtsam/base/Manifold.h:22,
from /usr/local/include/gtsam/geometry/BearingRange.h:21,
from /usr/local/include/gtsam/geometry/Pose2.h:23,
from RpgoReadG2o.cpp:10:
/usr/local/include/gtsam/base/OptionalJacobian.h:22:10: fatal error: Eigen/Dense: No such file or directory
22 | #include <Eigen/Dense>
| ^~~~~~~~~~~~~
compilation terminated.

I tried to copy the Eigen folder to both /usr/local/include and /usr/local/include/gtsam/base/ but I keep getting the same error.

Does anybody have any hint?

Hi! Thanks for sharing this very interesting project. I noted that installing this project with make install only installs the shared library binary, but not the headers (I see, for instance, that Kimera-RPGO contains forward declarations of this library).

Would it be possible to configure CMake to install the headers as well? It would make much easier to use this library within other projects.

Hello Yun Chang!

I am working on Robust Optimization for SLAM and was trying to use RPGO to compare with other methods on standard datasets ( M3500, INTEL, CSAIL, MIT ).

I used the snippet in the examples folder (RpgoReadG2o.cpp), but unfortunately, I am unable to converge to a reasonable solution on all datasets except M3500 ( classic, without additional noise ).
Being that I still didn't add any outliers to the dataset, I disabled outlier rejection as it was explained in #84, and used the same params for using only GNC.

These are the results that I obtained from the optimization, do you have any idea as to why these results and how to fix them?
The params I have used are :

• pcm_t = pcm_R = 10.0;
• params.setGncInlierCostThresholdsAtProbability(0.9999);

Thanks in advance for your time,
EO.

Hello, your Kimera-multi is really an excellent job!
I have a small question. I couldn't find the Robot Distributed Initialization section in the code. If I want to only initialize the initial pose of the robot, can I directly use the testMultirobotFrameAlign.cpp code?
Looking forward to your reply!

Hi，Yun Chang！ Thanks for your great work.
I have not carefully read the algorithm principles of related papers, but I want to quickly apply it to my LIO SLAM system. The following three pictures are my scene (single robot with many closed loops in the room), the original pose grpah, and the optimized pose graph using this cpp(RpgoReadG2oIncremental.cpp). As can be seen in the last picture, the optimized G2o has been completely biased. Could you please help me analyze the reason for this result?

here is my config.

 gtsam::NonlinearFactorGraph nfg = *gv.first;
  gtsam::Values values = *gv.second;
  std::cout << "factor size: " << nfg.size() << std::endl;

  if (useRPGO) {
    std::cout << "SET RPGO PARAMS!!!!" << std::endl;
    rpgoParams.setIncremental();
    rpgoParams.setPcm3DParams(-1, 1.0, Verbosity::VERBOSE);
    rpgoParams.setMultiRobotAlignMethod(MultiRobotAlignMethod::NONE);
    rpgo = KimeraRPGO::make_unique<RobustSolver>(rpgoParams);

    gtsam::Key current_key = nfg[0]->front();
    std::cout << "current key " << current_key << std::endl;
    gtsam::Values init_values;  // add first value with prior factor
    gtsam::NonlinearFactorGraph init_factors;
    init_values.insert(current_key, values.at<Pose3>(current_key));
    gtsam::PriorFactor<Pose3> prior_factor(
        current_key, values.at<Pose3>(current_key), noise_init_pose_prior);
    nfg.add(prior_factor);

    // separate to non loop closures and loop closure factors
    gtsam::NonlinearFactorGraph non_lc_factors, lc_factors;
    int i = 0, j = 0, k = 0;
    for (auto factor : nfg) {
      if (boost::dynamic_pointer_cast<gtsam::BetweenFactor<Pose3>>(factor)) {
        // specifically what outlier rejection handles
        gtsam::Key from_key = factor->front();
        gtsam::Key to_key = factor->back();
        if (from_key + 1 == to_key) {
          non_lc_factors.add(factor);  // odometry
          i++;
        } else {
          lc_factors.add(factor);  // loop closure
          j++;
        }
      } else {
        non_lc_factors.add(factor);  // not between so not lc
        k++;
      }
    }

    std::cout << "factor size: " << non_lc_factors.size() << std::endl;
    std::cout << "odom loop non-loc size: " << i << " " << j << " " << k
              << std::endl;

    // add non lc factors first
    rpgo->update(non_lc_factors, values);

    // Now add loop closure one by one
    for (auto loop_closure : lc_factors) {
      gtsam::NonlinearFactorGraph new_factors;
      new_factors.add(loop_closure);
      rpgo->update(new_factors, gtsam::Values(), false);
    }

    rpgo->saveData(
        dataSaverPtr->save_directory);  // tell pgo to save g2o result

here is my output, i use std::cout insted of the log function for some environmental issues.

 loop closure between keys %1% and %2% 1542 552
 total loop closures registered: %1% 188
number of inliers: %1% 119
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 136s 188 119
 loop closure between keys %1% and %2% 1544 554
 total loop closures registered: %1% 189
number of inliers: %1% 119
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 137s 189 119
 loop closure between keys %1% and %2% 1544 554
 total loop closures registered: %1% 190
number of inliers: %1% 119
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 140s 190 119
.....
....
.....
....
 loop closure between keys %1% and %2% 1610 621
 total loop closures registered: %1% 256
number of inliers: %1% 119
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 183s 256 119
 loop closure between keys %1% and %2% 1610 622
 total loop closures registered: %1% 257
number of inliers: %1% 119
 PCM spin took %1% milliseconds. Detected %2% total loop closures with %3% inliers 194s 257 119
SET RPGO PARAMS SUCCESS!!!!

thanks a lot!!!!!!!!!!!!!!!!!!!!!!!!!!!

Hello, it is an amazing work!
When I run the command: ./RpgoReadG2o 3d ../robot_a.g2o 1.0 1.0 ../ v in /build,
the following hint occured: setMaxIterations: changing the max nr of iters might lead to less accurate solutions and is not recommended!
So how can I get the correct result?

When running "catkin build" to build Kimera-PRGO, it occurs an error:
catkin_ws/src/Kimera-RPGO/src/RobustSolver.cpp:16:10: fatal error: gtsam/nonlinear/GncOptimizer.h: No such file or directory
It seems to be "gtsam" version problem, but I have no ideas to solve this.
How can I fix this?
My "gtsam" version is 4.1.0, using Ubuntu20.04 and Ros-Noetic.

Not sure what is going on:

• Installed Kimera-RPGO in 16.04
• with latest gtsam version

In file included from /usr/include/c++/5/unordered_map:35:0,
                 from /home/tonirv/code/Kimera-RPGO/KimeraRPGO/utils/graph_utils.h:7,
                 from /home/tonirv/code/Kimera-RPGO/KimeraRPGO/utils/graph_utils.cpp:3:
/usr/include/c++/5/bits/c++0x_warning.h:32:2: error: #error This file requires compiler and library support for the ISO C++ 2011 standard. This support must be enabled with the -std=c++11 or -std=gnu++11 compiler options.
 #error This file requires compiler and library support \
  ^
In file included from /usr/local/include/gtsam/inference/Key.h:25:0,
                 from /usr/local/include/gtsam/inference/Symbol.h:21,
                 from /home/tonirv/code/Kimera-RPGO/KimeraRPGO/utils/graph_utils.h:10,
                 from /home/tonirv/code/Kimera-RPGO/KimeraRPGO/utils/graph_utils.cpp:3:
/usr/local/include/gtsam/base/types.h:57:16: error: ‘uint64_t’ in namespace ‘std’ does not name a type
   typedef std::uint64_t Key;
                ^
In file included from /usr/local/include/gtsam/inference/Symbol.h:21:0,
                 from /home/tonirv/code/Kimera-RPGO/KimeraRPGO/utils/graph_utils.h:10,
                 from /home/tonirv/code/Kimera-RPGO/KimeraRPGO/utils/graph_utils.cpp:3:
/usr/local/include/gtsam/inference/Key.h:33:37: error: ‘Key’ was not declared in this scope
 typedef boost::function<std::string(Key)> KeyFormatter;
                                     ^
/usr/local/include/gtsam/inference/Key.h:33:41: error: template argument 1 is invalid
 typedef boost::function<std::string(Key)> KeyFormatter;
                                         ^
/usr/local/include/gtsam/inference/Key.h:36:47: error: ‘Key’ was not declared in this scope
 GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);

While running a unit test from Spark VIO I have a strange issue. It could be a problem in gtests, or in VIO, or here. But the issue popped up with RobustPGO so I'll put it here for now.

When I run the full test executable everything is fine. But if I just run a series of tests that have the PGO stuff (or even just the loop closure detector tests) then I a segfault. Running with gdb shows this to be the cause:

Thread 1 "testSparkVio" received signal SIGSEGV, Segmentation fault.
0x0000555555a505ec in std::default_delete<RobustPGO::OutlierRemoval>::operator() (this=0x5555563697c0, 
    __ptr=0x555556000000)
    at /usr/include/c++/7/bits/unique_ptr.h:78
78		delete __ptr;

So it looks like there is some problem with the teardown of RobustPGO class members. Again this could be a problem with the way I set them up in VIO or even just the test suite. Looking into it today, but @yunzc if you have any thoughts they'd be appreciated.