12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

More wrapping

release/4.3a0
Frank 2016-01-27 21:23:36 -08:00
parent 7b60c50297
commit 1c19b4e803
3 changed files with 33 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/navigation/NavState.h
View File

@ -75,18 +75,9 @@ public:
/// @name Component Access
/// @{
inline const Rot3& attitude() const {
return R_;
}
inline const Point3& position() const {
return t_;
}
inline const Velocity3& velocity() const {
return v_;
}
const Rot3& attitude(OptionalJacobian<3, 9> H) const;
const Point3& position(OptionalJacobian<3, 9> H) const;
const Velocity3& velocity(OptionalJacobian<3, 9> H) const;
const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const;
const Point3& position(OptionalJacobian<3, 9> H = boost::none) const;
const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const;
const Pose3 pose() const {
return Pose3(attitude(), position());

22
python/handwritten/navigation/ImuFactor.cpp
View File

@ -24,18 +24,29 @@
using namespace boost::python;
using namespace gtsam;
typedef gtsam::OptionalJacobian<9, 9> OptionalJacobian9;
typedef gtsam::OptionalJacobian<3, 9> OptionalJacobian39;
typedef gtsam::OptionalJacobian<9, 6> OptionalJacobian96;
typedef gtsam::OptionalJacobian<9, 9> OptionalJacobian9;
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(attitude_overloads, attitude, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(position_overloads, position, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(velocity_overloads, velocity, 0, 1)
void exportImuFactor() {
class_<OptionalJacobian9>("OptionalJacobian9", init<>());
class_<OptionalJacobian39>("OptionalJacobian39", init<>());
class_<OptionalJacobian96>("OptionalJacobian96", init<>());
class_<OptionalJacobian9>("OptionalJacobian9", init<>());
class_<NavState>("NavState", init<>())
// TODO(frank): overload with jacobians
// .def("attitude", &NavState::attitude)
// .def("position", &NavState::position)
// .def("velocity", &NavState::velocity)
.def("print", &NavState::print, print_overloads())
.def("attitude", &NavState::attitude,
attitude_overloads()[return_value_policy<copy_const_reference>()])
.def("position", &NavState::position,
position_overloads()[return_value_policy<copy_const_reference>()])
.def("velocity", &NavState::velocity,
velocity_overloads()[return_value_policy<copy_const_reference>()])
.def(repr(self))
.def("pose", &NavState::pose);
@ -77,6 +88,7 @@ void exportImuFactor() {
// NOTE(frank): Abstract classes need boost::noncopyable
class_<ImuFactor, bases<NonlinearFactor>, boost::shared_ptr<ImuFactor>>(
"ImuFactor")
.def("error", &ImuFactor::error)
.def(init<Key, Key, Key, Key, Key, const PreintegratedImuMeasurements&>())
.def(repr(self));
}

9
python/handwritten/nonlinear/NonlinearFactorGraph.cpp
View File

@ -28,6 +28,13 @@ using namespace gtsam;
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, NonlinearFactorGraph::print, 0, 1);
boost::shared_ptr<NonlinearFactor> getNonlinearFactor(
const NonlinearFactorGraph& graph, size_t idx) {
auto p = boost::dynamic_pointer_cast<NonlinearFactor>(graph.at(idx));
if (!p) throw std::runtime_error("No NonlinearFactor at requested index");
return p;
};
void exportNonlinearFactorGraph(){
typedef NonlinearFactorGraph::sharedFactor sharedFactor;
@ -44,4 +51,6 @@ void exportNonlinearFactorGraph(){
.def("print", &NonlinearFactorGraph::print, print_overloads(args("s")))
;
def("getNonlinearFactor", getNonlinearFactor);
}