Main Page
Related Pages
Modules
Namespaces
Namespace List
Namespace Members
All
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Functions
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Variables
_
a
b
c
d
e
f
g
h
i
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Typedefs
_
a
b
c
d
e
f
g
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
y
Enumerations
a
c
d
e
f
g
i
k
l
m
n
p
q
r
s
t
u
Enumerator
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
r
s
t
u
v
w
x
z
Classes
Class List
Class Hierarchy
Class Members
All
!
:
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
~
Functions
!
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
~
Variables
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Typedefs
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
z
Enumerations
a
b
c
d
f
k
l
m
n
o
p
r
s
t
v
z
Enumerator
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Related Functions
:
a
b
c
d
e
g
h
i
l
m
n
o
p
r
s
t
u
v
Files
File List
File Members
All
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Functions
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Variables
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Typedefs
a
b
c
d
e
f
g
h
i
k
l
m
n
o
p
q
r
s
t
u
v
x
z
Enumerations
Enumerator
b
c
e
f
g
i
l
m
n
o
p
r
s
t
u
v
x
y
z
Macros
_
a
b
c
d
e
f
g
h
i
k
l
m
n
o
p
q
r
s
t
u
v
w
x
z
Examples
python
gtsam
examples
Pose3SLAMExample_initializePose3Chordal.py
Go to the documentation of this file.
1
"""
2
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
3
Atlanta, Georgia 30332-0415
4
All Rights Reserved
5
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6
7
See LICENSE for the license information
8
9
Initialize PoseSLAM with Chordal init
10
Author: Luca Carlone, Frank Dellaert (python port)
11
"""
12
# pylint: disable=invalid-name, E1101
13
14
from
__future__
import
print_function
15
16
import
gtsam
17
import
numpy
as
np
18
19
20
def
main
():
21
"""Main runner."""
22
# Read graph from file
23
g2oFile =
gtsam.findExampleDataFile
(
"pose3example.txt"
)
24
25
is3D =
True
26
graph, initial =
gtsam.readG2o
(g2oFile, is3D)
27
28
# Add prior on the first key. TODO: assumes first key ios z
29
priorModel =
gtsam.noiseModel.Diagonal.Variances
(
30
np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
31
firstKey = initial.keys()[0]
32
graph.add(gtsam.PriorFactorPose3(0,
gtsam.Pose3
(), priorModel))
33
34
# Initializing Pose3 - chordal relaxation
35
initialization =
gtsam.InitializePose3.initialize
(graph)
36
37
print
(initialization)
38
39
40
if
__name__ ==
"__main__"
:
41
main
()
gtsam::readG2o
GraphAndValues readG2o(const std::string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition:
dataset.cpp:621
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition:
Matrix.cpp:145
gtsam::Pose3
Definition:
Pose3.h:37
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition:
dataset.cpp:70
gtsam.examples.Pose3SLAMExample_initializePose3Chordal.main
def main()
Definition:
Pose3SLAMExample_initializePose3Chordal.py:20
gtsam::InitializePose3::initialize
static Values initialize(const NonlinearFactorGraph &graph, const Values &givenGuess, bool useGradient=false)
Definition:
InitializePose3.cpp:296
gtsam::noiseModel::Diagonal::Variances
static shared_ptr Variances(const Vector &variances, bool smart=true)
Definition:
NoiseModel.cpp:275
gtsam
Author(s):
autogenerated on Wed Mar 19 2025 03:02:56