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

Merge branch 'feature/remove_deprecated_code' into verdant/replace-boost-optional-vals

release/4.3a0
Kartik Arcot 2023-01-21 11:42:23 -08:00
parent 1756114986 c71d07bbfd
commit 6cd59d755f
7 changed files with 56 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
examples/StereoVOExample_large.cpp
View File

@ -27,6 +27,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/utilities.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -113,7 +114,7 @@ int main(int argc, char** argv) {
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
Values pose_values = utilities::allPose3s(result);
pose_values.print("Final camera poses:\n");
return 0;

46
gtsam_unstable/examples/ConcurrentCalibration.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/utilities.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -35,12 +36,15 @@
using namespace std;
using namespace gtsam;
using symbol_shorthand::K;
using symbol_shorthand::L;
using symbol_shorthand::X;
int main(int argc, char** argv){
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1);
const auto model = noiseModel::Isotropic::Sigma(2,1);
string calibration_loc = findExampleDataFile("VO_calibration00s.txt");
string pose_loc = findExampleDataFile("VO_camera_poses00s.txt");
@ -54,13 +58,13 @@ int main(int argc, char** argv){
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
//create stereo camera calibration object
const Cal3_S2::shared_ptr K(new Cal3_S2(fx,fy,s,u0,v0));
const Cal3_S2::shared_ptr noisy_K(new Cal3_S2(fx*1.2,fy*1.2,s,u0-10,v0+10));
const Cal3_S2 true_K(fx,fy,s,u0,v0);
const Cal3_S2 noisy_K(fx*1.2,fy*1.2,s,u0-10,v0+10);
initial_estimate.insert(Symbol('K', 0), *noisy_K);
initial_estimate.insert(K(0), noisy_K);
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished());
graph.addPrior(Symbol('K', 0), *noisy_K, calNoise);
auto calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished());
graph.addPrior(K(0), noisy_K, calNoise);
ifstream pose_file(pose_loc.c_str());
@ -75,7 +79,7 @@ int main(int argc, char** argv){
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
}
noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
auto poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.addPrior(Symbol('x', pose_id), Pose3(m), poseNoise);
// camera and landmark keys
@ -83,22 +87,22 @@ int main(int argc, char** argv){
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
double uL, uR, v, X, Y, Z;
double uL, uR, v, _X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
// graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K);
while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) {
// graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model, X(x), L(l), K);
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0));
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(Point2(uL,v), model, X(x), L(l), K(0));
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
if (!initial_estimate.exists(L(l))) {
Pose3 camPose = initial_estimate.at<Pose3>(X(x));
//transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
initial_estimate.insert(Symbol('l', l), worldPoint);
Point3 worldPoint = camPose.transformFrom(Point3(_X, Y, Z));
initial_estimate.insert(L(l), worldPoint);
}
}
@ -123,7 +127,7 @@ int main(int argc, char** argv){
double currentError;
stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(Symbol('K',0)).vector().transpose() << endl;
stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(K(0)).vector().transpose() << endl;
// Iterative loop
@ -132,7 +136,7 @@ int main(int argc, char** argv){
currentError = optimizer.error();
optimizer.iterate();
stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(Symbol('K',0)).vector().transpose() << endl;
stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(K(0)).vector().transpose() << endl;
if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << optimizer.error() << endl;
} while(optimizer.iterations() < params.maxIterations &&
@ -142,13 +146,13 @@ int main(int argc, char** argv){
Values result = optimizer.values();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
Values pose_values = utilities::allPose3s(result);
pose_values.print("Final camera poses:\n");
Values(result.filter<Cal3_S2>()).print("Final K\n");
result.at<Cal3_S2>(K(0)).print("Final K\n");
noisy_K->print("Initial noisy K\n");
K->print("Initial correct K\n");
noisy_K.print("Initial noisy K\n");
true_K.print("Initial correct K\n");
return 0;
}

3
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -30,6 +30,7 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/utilities.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -114,7 +115,7 @@ int main(int argc, char** argv){
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
Values pose_values = utilities::allPose3s(result);
pose_values.print("Final camera poses:\n");
return 0;

10
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
View File

@ -234,9 +234,8 @@ int main(int argc, char** argv) {
}
}
countK = 0;
for(const auto it: result.filter<Point2>())
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
<< endl;
for (const auto& [key, point] : result.extract<Point2>())
os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl;
if (smart) {
for(size_t jj: ids) {
Point2 landmark = smartFactors[jj]->triangulate(result);
@ -257,9 +256,8 @@ int main(int argc, char** argv) {
// Write result to file
Values result = isam.calculateEstimate();
ofstream os("rangeResult.txt");
for(const auto it: result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl;
for (const auto& [key, pose] : result.extract<Pose2>())
os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl;
exit(0);
}

10
gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
View File

@ -202,13 +202,11 @@ int main(int argc, char** argv) {
// Write result to file
Values result = isam.calculateEstimate();
ofstream os2("rangeResultLM.txt");
for(const auto it: result.filter<Point2>())
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
<< endl;
for (const auto& [key, point] : result.extract<Point2>())
os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl;
ofstream os("rangeResult.txt");
for(const auto it: result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl;
for (const auto& [key, pose] : result.extract<Pose2>())
os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl;
exit(0);
}

24
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -29,6 +29,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/utilities.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -43,6 +44,7 @@
using namespace std;
using namespace gtsam;
using symbol_shorthand::X;
int main(int argc, char** argv){
@ -84,16 +86,16 @@ int main(int argc, char** argv){
if(add_initial_noise){
m(1,3) += (pose_id % 10)/10.0;
}
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
initial_estimate.insert(X(pose_id), Pose3(m));
}
Values initial_pose_values = initial_estimate.filter<Pose3>();
const auto initialPoses = initial_estimate.extract<Pose3>();
if (output_poses) {
init_pose3Out.open(init_poseOutput.c_str(), ios::out);
for (size_t i = 1; i <= initial_pose_values.size(); i++) {
for (size_t i = 1; i <= initialPoses.size(); i++) {
init_pose3Out
<< i << " "
<< initial_pose_values.at<Pose3>(Symbol('x', i)).matrix().format(
<< initialPoses.at(X(i)).matrix().format(
Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl;
}
}
@ -103,7 +105,7 @@ int main(int argc, char** argv){
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
double uL, uR, v, X, Y, Z;
double uL, uR, v, _X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
@ -112,21 +114,21 @@ int main(int argc, char** argv){
SmartFactor::shared_ptr factor(new SmartFactor(model));
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) {
if(current_l != l) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor(model));
current_l = l;
}
factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K);
factor->add(StereoPoint2(uL,uR,v), X(x), K);
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
Pose3 first_pose = initial_estimate.at<Pose3>(X(1));
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
graph.emplace_shared<NonlinearEquality<Pose3> >(X(1),first_pose);
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
@ -138,13 +140,13 @@ int main(int argc, char** argv){
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
Values pose_values = utilities::allPose3s(result);
pose_values.print("Final camera poses:\n");
if(output_poses){
pose3Out.open(poseOutput.c_str(),ios::out);
for(size_t i = 1; i<=pose_values.size(); i++){
pose3Out << i << " " << pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
pose3Out << i << " " << pose_values.at<Pose3>(X(i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
" ", " ")) << endl;
}
cout << "Writing output" << endl;

13
timing/timeLago.cpp
View File

@ -10,8 +10,8 @@
* -------------------------------------------------------------------------- */
/**
* @file timeVirtual.cpp
* @brief Time the overhead of using virtual destructors and methods
* @file timeLago.cpp
* @brief Time the LAGO initialization method
* @author Richard Roberts
* @date Dec 3, 2010
*/
@ -36,16 +36,15 @@ int main(int argc, char *argv[]) {
Values::shared_ptr solution;
NonlinearFactorGraph::shared_ptr g;
string inputFile = findExampleDataFile("w10000");
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
auto model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
boost::tie(g, solution) = load2D(inputFile, model);
// add noise to create initial estimate
Values initial;
Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished());
auto noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished());
Sampler sampler(noise);
for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: poses)
initial.insert(it.key, it.value.retract(sampler.sample()));
for(const auto& [key,pose]: solution->extract<Pose2>())
initial.insert(key, pose.retract(sampler.sample()));
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //