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

minor variable renaming

release/4.3a0
Duy-Nguyen Ta 2012-06-05 04:10:13 +00:00
parent c1d35ac486
commit 76a9e651cb
3 changed files with 20 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/VisualISAMExample.cpp
View File

@ -72,8 +72,8 @@ int main(int argc, char* argv[]) {
initials.insert(X(1), pose0Init*odoMeasurement);
// Initial values for the landmarks, simulated with Gaussian noise
for (size_t j=0; j<data.landmarks.size(); ++j)
initials.insert(L(j), data.landmarks[j] + Point3(data.noiseL->sample()));
for (size_t j=0; j<data.points.size(); ++j)
initials.insert(L(j), data.points[j] + Point3(data.noiseL->sample()));
// Update ISAM the first time and obtain the current estimate
isam.update(newFactors, initials);

27
examples/VisualSLAMData.h
View File

@ -40,7 +40,7 @@ struct VisualSLAMExampleData {
gtsam::shared_ptrK sK; // camera calibration parameters
std::vector<gtsam::Pose3> poses; // ground-truth camera poses
gtsam::Pose3 odometry; // ground-truth odometry between 2 consecutive poses (simulated data for iSAM)
std::vector<gtsam::Point3> landmarks; // ground-truth landmarks
std::vector<gtsam::Point3> points; // ground-truth landmarks
std::map<int, vector<gtsam::Point2> > z; // 2D measurements of landmarks in each camera frame
gtsam::SharedDiagonal noiseZ; // measurement noise (noiseModel::Isotropic::Sigma(2, 5.0f));
gtsam::SharedDiagonal noiseX; // noise for camera poses
@ -49,14 +49,14 @@ struct VisualSLAMExampleData {
static const VisualSLAMExampleData generate() {
VisualSLAMExampleData data;
// Landmarks (ground truth)
data.landmarks.push_back(gtsam::Point3(10.0,10.0,10.0));
data.landmarks.push_back(gtsam::Point3(-10.0,10.0,10.0));
data.landmarks.push_back(gtsam::Point3(-10.0,-10.0,10.0));
data.landmarks.push_back(gtsam::Point3(10.0,-10.0,10.0));
data.landmarks.push_back(gtsam::Point3(10.0,10.0,-10.0));
data.landmarks.push_back(gtsam::Point3(-10.0,10.0,-10.0));
data.landmarks.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
data.landmarks.push_back(gtsam::Point3(10.0,-10.0,-10.0));
data.points.push_back(gtsam::Point3(10.0,10.0,10.0));
data.points.push_back(gtsam::Point3(-10.0,10.0,10.0));
data.points.push_back(gtsam::Point3(-10.0,-10.0,10.0));
data.points.push_back(gtsam::Point3(10.0,-10.0,10.0));
data.points.push_back(gtsam::Point3(10.0,10.0,-10.0));
data.points.push_back(gtsam::Point3(-10.0,10.0,-10.0));
data.points.push_back(gtsam::Point3(-10.0,-10.0,-10.0));
data.points.push_back(gtsam::Point3(10.0,-10.0,-10.0));
// Camera calibration parameters
data.sK = gtsam::shared_ptrK(new gtsam::Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
@ -65,8 +65,7 @@ struct VisualSLAMExampleData {
int n = 8;
double theta = 0.0;
double r = 30.0;
for (int i=0; i<n; ++i) {
theta += 2*M_PI/n;
for (int i=0; i<n; ++i, theta += 2*M_PI/n) {
data.poses.push_back(gtsam::Pose3(
gtsam::Rot3(-sin(theta), 0.0, -cos(theta),
cos(theta), 0.0, -sin(theta),
@ -78,12 +77,12 @@ struct VisualSLAMExampleData {
// Simulated measurements with Gaussian noise
data.noiseZ = gtsam::sharedSigma(2, 1.0);
for (size_t i=0; i<data.poses.size(); ++i) {
for (size_t j=0; j<data.landmarks.size(); ++j) {
for (size_t j=0; j<data.points.size(); ++j) {
gtsam::SimpleCamera camera(data.poses[i], *data.sK);
data.z[i].push_back(camera.project(data.landmarks[j]) + gtsam::Point2(data.noiseZ->sample()));
data.z[i].push_back(camera.project(data.points[j]) + gtsam::Point2(data.noiseZ->sample()));
}
}
data.noiseX = gtsam::sharedSigmas(gtsam::Vector_(6, 0.01, 0.01, 0.01, 0.1, 0.1, 0.1));
data.noiseX = gtsam::sharedSigmas(gtsam::Vector_(6, 0.001, 0.001, 0.001, 0.1, 0.1, 0.1));
data.noiseL = gtsam::sharedSigma(3, 0.1);
return data;

9
examples/VisualSLAMExample.cpp
View File

@ -39,18 +39,19 @@ int main(int argc, char* argv[]) {
/* 2. Add factors to the graph */
// 2a. Measurement factors
for (size_t i=0; i<data.poses.size(); ++i) {
for (size_t j=0; j<data.landmarks.size(); ++j)
for (size_t j=0; j<data.points.size(); ++j)
graph.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
}
// 2b. Prior factor for the first pose to resolve ambiguity (not needed for LevenbergMarquardtOptimizer)
// 2b. Prior factor for the first pose and point to constraint the system
graph.addPosePrior(X(0), data.poses[0], data.noiseX);
graph.addPointPrior(L(0), data.points[0], data.noiseL);
/* 3. Initial estimates for variable nodes, simulated by Gaussian noises */
Values initial;
for (size_t i=0; i<data.poses.size(); ++i)
initial.insert(X(i), data.poses[i]*Pose3::Expmap(data.noiseX->sample()));
for (size_t j=0; j<data.landmarks.size(); ++j)
initial.insert(L(j), data.landmarks[j] + Point3(data.noiseL->sample()));
for (size_t j=0; j<data.points.size(); ++j)
initial.insert(L(j), data.points[j] + Point3(data.noiseL->sample()));
initial.print("Intial Estimates: ");
/* 4. Optimize the graph and print results */