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

Merged in feature/more_python (pull request #209) 

Feature/more_python
release/4.3a0
Frank Dellaert 2016-01-28 08:08:19 -08:00
parent a0f512edd4 1d62faa5a5
commit d5ca4c1e0c
7 changed files with 138 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
python/gtsam_examples/VisualISAM2Example.py
View File

@ -3,19 +3,23 @@ from __future__ import print_function
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
import time # for sleep()
import time # for sleep()
import gtsam
from gtsam_examples import SFMdata
import gtsam_utils
# shorthand symbols:
X = lambda i: int(gtsam.Symbol('x', i))
L = lambda j: int(gtsam.Symbol('l', j))
def visual_ISAM2_plot(poses, points, result):
# VisualISAMPlot plots current state of ISAM2 object
# Author: Ellon Paiva
# Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
# Declare an id for the figure
fignum = 0;
fignum = 0
fig = plt.figure(fignum)
ax = fig.gca(projection='3d')
@ -23,33 +27,31 @@ def visual_ISAM2_plot(poses, points, result):
# Plot points
# Can't use data because current frame might not see all points
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()); # TODO - this is slow
# gtsam.plot3DPoints(result, [], marginals);
gtsam_utils.plot3DPoints(fignum, result, 'rx');
# marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # TODO - this is slow
# gtsam.plot3DPoints(result, [], marginals)
gtsam_utils.plot3DPoints(fignum, result, 'rx')
# Plot cameras
M = 0;
while result.exists(int(gtsam.Symbol('x',M))):
ii = int(gtsam.Symbol('x',M));
pose_i = result.pose3_at(ii);
gtsam_utils.plotPose3(fignum, pose_i, 10);
M = M + 1;
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
gtsam_utils.plotPose3(fignum, pose_i, 10)
i += 1
# draw
ax.set_xlim3d(-40, 40)
ax.set_ylim3d(-40, 40)
ax.set_zlim3d(-40, 40)
plt.ion()
plt.show()
plt.draw()
plt.pause(1)
def visual_ISAM2_example():
plt.ion()
# Define the camera calibration parameters
K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
@ -78,29 +80,29 @@ def visual_ISAM2_example():
for j, point in enumerate(points):
camera = gtsam.PinholeCameraCal3_S2(pose, K)
measurement = camera.project(point)
graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, int(gtsam.Symbol('x', i)), int(gtsam.Symbol('l', j)), K))
graph.push_back(gtsam.GenericProjectionFactorCal3_S2(measurement, measurementNoise, X(i), L(j), K))
# Add an initial guess for the current pose
# Intentionally initialize the variables off from the ground truth
initialEstimate.insert(int(gtsam.Symbol('x', i)), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
initialEstimate.insert(X(i), pose.compose(gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))
# If this is the first iteration, add a prior on the first pose to set the coordinate frame
# and a prior on the first landmark to set the scale
# Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
# adding it to iSAM.
if( i == 0):
if(i == 0):
# Add a prior on pose x0
poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(gtsam.PriorFactorPose3(int(gtsam.Symbol('x', 0)), poses[0], poseNoise))
poseNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], poseNoise))
# Add a prior on landmark l0
pointNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
graph.push_back(gtsam.PriorFactorPoint3(int(gtsam.Symbol('l', 0)), points[0], pointNoise)) # add directly to graph
graph.push_back(gtsam.PriorFactorPoint3(L(0), points[0], pointNoise)) # add directly to graph
# Add initial guesses to all observed landmarks
# Intentionally initialize the variables off from the ground truth
for j, point in enumerate(points):
initialEstimate.insert(int(gtsam.Symbol('l', j)), point + gtsam.Point3(-0.25, 0.20, 0.15));
initialEstimate.insert(L(j), point + gtsam.Point3(-0.25, 0.20, 0.15))
else:
# Update iSAM with the new factors
isam.update(graph, initialEstimate)
@ -108,23 +110,23 @@ def visual_ISAM2_example():
# If accuracy is desired at the expense of time, update(*) can be called additional times
# to perform multiple optimizer iterations every step.
isam.update()
currentEstimate = isam.calculate_estimate();
print( "****************************************************" )
print( "Frame", i, ":" )
for j in range(i+1):
print( gtsam.Symbol('x',j) )
print( currentEstimate.pose3_at(int(gtsam.Symbol('x',j))) )
currentEstimate = isam.calculate_estimate()
print("****************************************************")
print("Frame", i, ":")
for j in range(i + 1):
print(X(j), ":", currentEstimate.atPose3(X(j)))
for j in range(len(points)):
print( gtsam.Symbol('l',j) )
print( currentEstimate.point3_at(int(gtsam.Symbol('l',j))) )
print(L(j), ":", currentEstimate.atPoint3(L(j)))
visual_ISAM2_plot(poses, points, currentEstimate);
time.sleep(1)
visual_ISAM2_plot(poses, points, currentEstimate)
# Clear the factor graph and values for the next iteration
graph.resize(0);
initialEstimate.clear();
graph.resize(0)
initialEstimate.clear()
plt.ioff()
plt.show()
if __name__ == '__main__':
visual_ISAM2_example()

2
python/gtsam_utils/__init__.py
View File

@ -1 +1 @@
from ._plot import *
from .plot import *

59
python/gtsam_utils/plot.py Normal file
View File

@ -0,0 +1,59 @@
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D as _Axes3D
def plotPoint3(fignum, point, linespec):
fig = plt.figure(fignum)
ax = fig.gca(projection='3d')
ax.plot([point.x()], [point.y()], [point.z()], linespec)
def plot3DPoints(fignum, values, linespec, marginals=None):
# PLOT3DPOINTS Plots the Point3's in a values, with optional covariances
# Finds all the Point3 objects in the given Values object and plots them.
# If a Marginals object is given, this function will also plot marginal
# covariance ellipses for each point.
keys = values.keys()
# Plot points and covariance matrices
for key in keys:
try:
p = values.atPoint3(key);
# if haveMarginals
# P = marginals.marginalCovariance(key);
# gtsam.plotPoint3(p, linespec, P);
# else
plotPoint3(fignum, p, linespec);
except RuntimeError:
continue
# I guess it's not a Point3
def plotPose3(fignum, pose, axisLength=0.1):
# get figure object
fig = plt.figure(fignum)
ax = fig.gca(projection='3d')
# get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global
C = pose.translation().vector()
# draw the camera axes
xAxis = C + gRp[:, 0] * axisLength
L = np.append(C[np.newaxis], xAxis[np.newaxis], axis=0)
ax.plot(L[:, 0], L[:, 1], L[:, 2], 'r-')
yAxis = C + gRp[:, 1] * axisLength
L = np.append(C[np.newaxis], yAxis[np.newaxis], axis=0)
ax.plot(L[:, 0], L[:, 1], L[:, 2], 'g-')
zAxis = C + gRp[:, 2] * axisLength
L = np.append(C[np.newaxis], zAxis[np.newaxis], axis=0)
ax.plot(L[:, 0], L[:, 1], L[:, 2], 'b-')
# # plot the covariance
# if (nargin>2) && (~isempty(P))
# pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame
# gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
# gtsam.covarianceEllipse3D(C,gPp);
# end

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

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -11,7 +11,7 @@
/**
* @brief exports NonlinearFactorGraph class to python
* @author Andrew Melim
* @author Andrew Melim
* @author Ellon Paiva Mendes (LAAS-CNRS)
**/
@ -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;
@ -36,7 +43,7 @@ void exportNonlinearFactorGraph(){
void (NonlinearFactorGraph::*add1)(const sharedFactor&) = &NonlinearFactorGraph::add;
class_<NonlinearFactorGraph>("NonlinearFactorGraph", init<>())
.def("size",&NonlinearFactorGraph::size)
.def("size",&NonlinearFactorGraph::size)
.def("push_back", push_back1)
.def("add", add1)
.def("resize", &NonlinearFactorGraph::resize)
@ -44,4 +51,6 @@ void exportNonlinearFactorGraph(){
.def("print", &NonlinearFactorGraph::print, print_overloads(args("s")))
;
def("getNonlinearFactor", getNonlinearFactor);
}

76
python/handwritten/nonlinear/Values.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -26,61 +26,17 @@
#include "gtsam/geometry/Point3.h"
#include "gtsam/geometry/Rot3.h"
#include "gtsam/geometry/Pose3.h"
#include "gtsam/navigation/ImuBias.h"
using namespace boost::python;
using namespace gtsam;
/** The function ValuesAt is a workaround to be able to call the correct templated version
* of Values::at. Without it, python would only try to match the last 'at' metho defined
* below. With this wrapper function we can call 'at' in python passing an extra type,
* which will define the type to be returned. Example:
*
* >>> import gtsam
* >>> v = gtsam.nonlinear.Values()
* >>> v.insert(1,gtsam.geometry.Point3())
* >>> v.insert(2,gtsam.geometry.Rot3())
* >>> v.insert(3,gtsam.geometry.Pose3())
* >>> v.at(1,gtsam.geometry.Point3())
* >>> v.at(2,gtsam.geometry.Rot3())
* >>> v.at(3,gtsam.geometry.Pose3())
*
* A more 'pythonic' way I think would be to not use this function and define different
* 'at' methods below using the name of the type in the function name, like:
*
* .def("point3_at", &Values::at<Point3>, return_internal_reference<>())
* .def("rot3_at", &Values::at<Rot3>, return_internal_reference<>())
* .def("pose3_at", &Values::at<Pose3>, return_internal_reference<>())
*
* and then they could be accessed from python as
*
* >>> import gtsam
* >>> v = gtsam.nonlinear.Values()
* >>> v.insert(1,gtsam.geometry.Point3())
* >>> v.insert(2,gtsam.geometry.Rot3())
* >>> v.insert(3,gtsam.geometry.Pose3())
* >>> v.point3_at(1)
* >>> v.rot3_at(2)
* >>> v.pose3_at(3)
*
* In fact, I just saw the pythonic way sounds more clear, so I'm sticking with this and
* leaving the comments here for future reference. I'm using the PEP0008 for method naming.
* See: https://www.python.org/dev/peps/pep-0008/#function-and-method-arguments
*/
// template<typename T>
// const T & ValuesAt( const Values & v, Key j, T /*type*/)
// {
// return v.at<T>(j);
// }
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Values::print, 0, 1);
void exportValues(){
// NOTE: Apparently the class 'Value'' is deprecated, so the commented lines below
// will compile, but are useless in the python wrapper. We need to use specific
// 'at' and 'insert' methods for each type.
// const Value& (Values::*at1)(Key) const = &Values::at;
// void (Values::*insert1)(Key, const Value&) = &Values::insert;
typedef imuBias::ConstantBias Bias;
bool (Values::*exists1)(Key) const = &Values::exists;
void (Values::*insert_point2)(Key, const gtsam::Point2&) = &Values::insert;
void (Values::*insert_rot2) (Key, const gtsam::Rot2&) = &Values::insert;
@ -88,7 +44,8 @@ void exportValues(){
void (Values::*insert_point3)(Key, const gtsam::Point3&) = &Values::insert;
void (Values::*insert_rot3) (Key, const gtsam::Rot3&) = &Values::insert;
void (Values::*insert_pose3) (Key, const gtsam::Pose3&) = &Values::insert;
void (Values::*insert_bias) (Key, const Bias&) = &Values::insert;
void (Values::*insert_vector3) (Key, const gtsam::Vector3&) = &Values::insert;
class_<Values>("Values", init<>())
.def(init<Values>())
@ -101,23 +58,22 @@ void exportValues(){
.def("print", &Values::print, print_overloads(args("s")))
.def("size", &Values::size)
.def("swap", &Values::swap)
// NOTE: Following commented lines add useless methods on Values
// .def("insert", insert1)
// .def("at", at1, return_value_policy<copy_const_reference>())
.def("insert", insert_point2)
.def("insert", insert_rot2)
.def("insert", insert_pose2)
.def("insert", insert_point3)
.def("insert", insert_rot3)
.def("insert", insert_pose3)
// NOTE: The following commented lines are another way of specializing the return type.
// See long comment above.
// .def("at", &ValuesAt<Point3>, return_internal_reference<>())
// .def("at", &ValuesAt<Rot3>, return_internal_reference<>())
// .def("at", &ValuesAt<Pose3>, return_internal_reference<>())
.def("point3_at", &Values::at<Point3>, return_value_policy<copy_const_reference>())
.def("rot3_at", &Values::at<Rot3>, return_value_policy<copy_const_reference>())
.def("pose3_at", &Values::at<Pose3>, return_value_policy<copy_const_reference>())
.def("insert", insert_bias)
.def("insert", insert_vector3)
.def("atPoint2", &Values::at<Point2>, return_value_policy<copy_const_reference>())
.def("atRot2", &Values::at<Rot2>, return_value_policy<copy_const_reference>())
.def("atPose2", &Values::at<Pose2>, return_value_policy<copy_const_reference>())
.def("atPoint3", &Values::at<Point3>, return_value_policy<copy_const_reference>())
.def("atRot3", &Values::at<Rot3>, return_value_policy<copy_const_reference>())
.def("atPose3", &Values::at<Pose3>, return_value_policy<copy_const_reference>())
.def("atConstantBias", &Values::at<Bias>, return_value_policy<copy_const_reference>())
.def("atVector3", &Values::at<Vector3>, return_value_policy<copy_const_reference>())
.def("exists", exists1)
.def("keys", &Values::keys)
;

18
python/handwritten/slam/BetweenFactor.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -11,7 +11,7 @@
/**
* @brief wraps BetweenFactor for several values to python
* @author Andrew Melim
* @author Andrew Melim
* @author Ellon Paiva Mendes (LAAS-CNRS)
**/
@ -33,17 +33,17 @@ using namespace gtsam;
using namespace std;
// template<class VALUE>
// template<class T>
// void exportBetweenFactor(const std::string& name){
// class_<VALUE>(name, init<>())
// .def(init<Key, Key, VALUE, SharedNoiseModel>())
// class_<T>(name, init<>())
// .def(init<Key, Key, T, SharedNoiseModel>())
// ;
// }
#define BETWEENFACTOR(VALUE) \
class_< BetweenFactor<VALUE>, bases<NonlinearFactor>, boost::shared_ptr< BetweenFactor<VALUE> > >("BetweenFactor"#VALUE) \
.def(init<Key,Key,VALUE,noiseModel::Base::shared_ptr>()) \
.def("measured", &BetweenFactor<VALUE>::measured, return_internal_reference<>()) \
#define BETWEENFACTOR(T) \
class_< BetweenFactor<T>, bases<NonlinearFactor>, boost::shared_ptr< BetweenFactor<T> > >("BetweenFactor"#T) \
.def(init<Key,Key,T,noiseModel::Base::shared_ptr>()) \
.def("measured", &BetweenFactor<T>::measured, return_internal_reference<>()) \
;
void exportBetweenFactors()

7
python/handwritten/slam/PriorFactor.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -11,7 +11,7 @@
/**
* @brief wraps PriorFactor for several values to python
* @author Andrew Melim
* @author Andrew Melim
* @author Ellon Paiva Mendes (LAAS-CNRS)
**/
@ -54,4 +54,5 @@ void exportPriorFactors()
PRIORFACTOR(Point3)
PRIORFACTOR(Rot3)
PRIORFACTOR(Pose3)
}
PRIORFACTOR(Vector3)
}