Skip to content

Commit

Permalink
Removed filter from examples and timing scripts
Browse files Browse the repository at this point in the history
  • Loading branch information
dellaert committed Jan 21, 2023
1 parent 1ecba12 commit c71d07b
Show file tree
Hide file tree
Showing 7 changed files with 56 additions and 53 deletions.
3 changes: 2 additions & 1 deletion examples/StereoVOExample_large.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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;
Expand Down
46 changes: 25 additions & 21 deletions gtsam_unstable/examples/ConcurrentCalibration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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");
Expand All @@ -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());
Expand All @@ -75,30 +79,30 @@ 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
size_t x, l;

// 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);
}
}

Expand All @@ -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
Expand All @@ -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 &&
Expand All @@ -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 changes: 2 additions & 1 deletion gtsam_unstable/examples/SmartProjectionFactorExample.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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;
Expand Down
10 changes: 4 additions & 6 deletions gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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 changes: 4 additions & 6 deletions gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 changes: 13 additions & 11 deletions gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -43,6 +44,7 @@

using namespace std;
using namespace gtsam;
using symbol_shorthand::X;

int main(int argc, char** argv){

Expand Down Expand Up @@ -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;
}
}
Expand All @@ -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;

Expand All @@ -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;
Expand All @@ -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;
Expand Down
13 changes: 6 additions & 7 deletions timing/timeLago.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand All @@ -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 = //
Expand Down

0 comments on commit c71d07b

Please sign in to comment.