Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ignore stubgen errors and wrap TriangulationFactor #1846

Merged
merged 9 commits into from
Sep 26, 2024
10 changes: 6 additions & 4 deletions cmake/Config.cmake.in
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,12 @@ endif()
# Find dependencies, required by cmake exported targets:
include(CMakeFindDependencyMacro)
# Allow using cmake < 3.8
if(${CMAKE_VERSION} VERSION_LESS "3.8.0")
find_package(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
else()
find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
if (@GTSAM_ENABLE_BOOST_SERIALIZATION@ OR @GTSAM_USE_BOOST_FEATURES@)
ProfFan marked this conversation as resolved.
Show resolved Hide resolved
if(${CMAKE_VERSION} VERSION_LESS "3.8.0")
find_package(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
else()
find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@)
endif()
endif()

if(@GTSAM_USE_TBB@)
Expand Down
8 changes: 4 additions & 4 deletions gtsam/inference/Factor.h
Original file line number Diff line number Diff line change
Expand Up @@ -24,13 +24,13 @@
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif
#include <memory>
#include <algorithm>

#include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/types.h>
#include <gtsam/inference/Key.h>

#include <algorithm>
#include <memory>

namespace gtsam {

/// Define collection types:
Expand Down
5 changes: 2 additions & 3 deletions gtsam/slam/TriangulationFactor.h
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ class TriangulationFactor: public NoiseModelFactorN<Point3> {

/// CAMERA type
using Camera = CAMERA;
/// shorthand for measurement type, e.g. Point2 or StereoPoint2
using Measurement = typename CAMERA::Measurement;

protected:

Expand All @@ -43,9 +45,6 @@ class TriangulationFactor: public NoiseModelFactorN<Point3> {
/// shorthand for this class
using This = TriangulationFactor<CAMERA>;

/// shorthand for measurement type, e.g. Point2 or StereoPoint2
using Measurement = typename CAMERA::Measurement;

// Keep a copy of measurement and calibration for I/O
const CAMERA camera_; ///< CAMERA in which this landmark was seen
const Measurement measured_; ///< 2D measurement
Expand Down
43 changes: 41 additions & 2 deletions gtsam/slam/slam.i
Original file line number Diff line number Diff line change
Expand Up @@ -347,11 +347,50 @@ virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {

gtsam::Vector evaluateError(const T& R1, const T& R2);
};


#include <gtsam/slam/TriangulationFactor.h>
template <CAMERA>
virtual class TriangulationFactor : gtsam::NoiseModelFactor {
TriangulationFactor();
TriangulationFactor(const CAMERA& camera, const gtsam::This::Measurement& measured,
const gtsam::noiseModel::Base* model, gtsam::Key pointKey,
bool throwCheirality = false,
bool verboseCheirality = false);

void print(const string& s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const This& p, double tol = 1e-9) const;

gtsam::Vector evaluateError(const gtsam::Point3& point) const;

const gtsam::This::Measurement& measured() const;
};
typedef gtsam::TriangulationFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>>
TriangulationFactorCal3_S2;
typedef gtsam::TriangulationFactor<gtsam::PinholeCamera<gtsam::Cal3DS2>>
TriangulationFactorCal3DS2;
typedef gtsam::TriangulationFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>>
TriangulationFactorCal3Bundler;
typedef gtsam::TriangulationFactor<gtsam::PinholeCamera<gtsam::Cal3Fisheye>>
TriangulationFactorCal3Fisheye;
typedef gtsam::TriangulationFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>>
TriangulationFactorCal3Unified;

typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3_S2>>
TriangulationFactorPoseCal3_S2;
typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3DS2>>
TriangulationFactorPoseCal3DS2;
typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3Bundler>>
TriangulationFactorPoseCal3Bundler;
typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3Fisheye>>
TriangulationFactorPoseCal3Fisheye;
typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3Unified>>
TriangulationFactorPoseCal3Unified;

#include <gtsam/slam/lago.h>
namespace lago {
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true);
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess);
}

} // namespace gtsam
4 changes: 2 additions & 2 deletions python/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -257,7 +257,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
COMMAND
${CMAKE_COMMAND} -E env
"PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}"
pybind11-stubgen -o . --enum-class-locations \"KernelFunctionType|NoiseFormat:gtsam.gtsam\" --enum-class-locations \"OrderingType:gtsam.gtsam.Ordering\" --numpy-array-use-type-var gtsam_unstable
ProfFan marked this conversation as resolved.
Show resolved Hide resolved
pybind11-stubgen -o . --enum-class-locations \"KernelFunctionType|NoiseFormat:gtsam.gtsam\" --enum-class-locations \"OrderingType:gtsam.gtsam.Ordering\" --numpy-array-use-type-var --ignore-all-errors gtsam_unstable
DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} ${GTSAM_PYTHON_UNSTABLE_TARGET}
WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/"
)
Expand All @@ -284,7 +284,7 @@ add_custom_target(
COMMAND
${CMAKE_COMMAND} -E env
"PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}"
pybind11-stubgen -o . --enum-class-locations \"KernelFunctionType|NoiseFormat:gtsam.gtsam\" --enum-class-locations \"OrderingType:gtsam.gtsam.Ordering\" --numpy-array-use-type-var gtsam
pybind11-stubgen -o . --enum-class-locations \"KernelFunctionType|NoiseFormat:gtsam.gtsam\" --enum-class-locations \"OrderingType:gtsam.gtsam.Ordering\" --numpy-array-use-type-var --ignore-all-errors gtsam
DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} ${GTSAM_PYTHON_TARGET}
WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/"
)
Expand Down
Loading