moved StereoFactor from VO to gtsam
Chris Beall
parent
3f1029483c
commit
651db9240b
|
|
@ -49,6 +49,10 @@ check_PROGRAMS += tests/testPose3Factor tests/testPose3Values tests/testPose3SLA
|
|||
sources += visualSLAM.cpp
|
||||
check_PROGRAMS += tests/testVSLAMFactor tests/testVSLAMGraph tests/testVSLAMValues
|
||||
|
||||
# StereoFactor
|
||||
headers += StereoFactor.h
|
||||
check_PROGRAMS += tests/testStereoFactor
|
||||
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
# Create a libtool library that is not installed
|
||||
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
|
||||
|
|
|
|||
|
|
@ -0,0 +1,111 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file StereoFactor.h
|
||||
* @brief A non-linear factor for stereo measurements
|
||||
* @author Alireza Fathi
|
||||
* @author Chris Beall
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/visualSLAM.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace gtsam::visualSLAM;
|
||||
|
||||
class StereoFactor: public NonlinearFactor2<Values, PoseKey, PointKey>, Testable<StereoFactor> {
|
||||
private:
|
||||
|
||||
// Keep a copy of measurement and calibration for I/O
|
||||
StereoPoint2 z_;
|
||||
boost::shared_ptr<Cal3_S2> K_;
|
||||
double baseline_;
|
||||
|
||||
public:
|
||||
|
||||
// shorthand for base class type
|
||||
typedef NonlinearFactor2<Values, PoseKey, PointKey> Base;
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<StereoFactor> shared_ptr;
|
||||
|
||||
/**
|
||||
* Default constructor
|
||||
*/
|
||||
StereoFactor() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param z is the 2 dimensional location of point in image (the measurement)
|
||||
* @param sigma is the standard deviation
|
||||
* @param cameraFrameNumber is basically the frame number
|
||||
* @param landmarkNumber is the index of the landmark
|
||||
* @param K the constant calibration
|
||||
*/
|
||||
StereoFactor(const StereoPoint2& z, const SharedGaussian& model, PoseKey j_pose, PointKey j_landmark, const shared_ptrK& K, double baseline) :
|
||||
Base(model, j_pose, j_landmark), z_(z), K_(K), baseline_(baseline) {}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
*/
|
||||
void print(const std::string& s) const {
|
||||
printf("%s %s %s\n", s.c_str(), ((string) key1_).c_str(),
|
||||
((string) key2_).c_str());
|
||||
z_.print(s+".z");
|
||||
}
|
||||
|
||||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const StereoFactor& f, double tol) const {
|
||||
const StereoFactor* p = dynamic_cast<const StereoFactor*>(&f);
|
||||
if (p == NULL) goto fail;
|
||||
//if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail;
|
||||
if (!z_.equals(p->z_,tol)) goto fail;
|
||||
return true;
|
||||
|
||||
fail:
|
||||
print("actual");
|
||||
p->print("expected");
|
||||
return false;
|
||||
}
|
||||
|
||||
/** h(x)-z */
|
||||
Vector evaluateError(const Pose3& pose, const Point3& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
|
||||
const Cal3_S2& K = *K_;
|
||||
StereoCamera stereoCam(pose, K, baseline_);
|
||||
|
||||
return (stereoCam.project(point,H1,H2) - z_).vector();
|
||||
}
|
||||
|
||||
StereoPoint2 z(){return z_;}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
//ar & BOOST_SERIALIZATION_NVP(key1_);
|
||||
//ar & BOOST_SERIALIZATION_NVP(key2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(z_);
|
||||
ar & BOOST_SERIALIZATION_NVP(K_);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,94 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testStereoFactor.cpp
|
||||
* @brief Unit test for StereoFactor
|
||||
* single camera
|
||||
* @author Chris Beall
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/geometry/StereoCamera.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
||||
#include <gtsam/slam/StereoFactor.h>
|
||||
|
||||
#include <gtsam/inference/graph-inl.h>
|
||||
#include <gtsam/slam/visualSLAM.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace gtsam::visualSLAM;
|
||||
using namespace boost;
|
||||
typedef NonlinearOptimizer<Graph, Values> Optimizer;
|
||||
|
||||
Pose3 camera1(Matrix_(3,3,
|
||||
1., 0., 0.,
|
||||
0.,-1., 0.,
|
||||
0., 0.,-1.
|
||||
),
|
||||
Point3(0,0,6.25));
|
||||
|
||||
Cal3_S2 K(1500, 1500, 0, 320, 240);
|
||||
StereoCamera stereoCam(Pose3(), K, 0.5);
|
||||
|
||||
// point X Y Z in meters
|
||||
Point3 p(0, 0, 5);
|
||||
static SharedGaussian sigma(noiseModel::Unit::Create(1));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( StereoFactor, singlePoint)
|
||||
{
|
||||
//Cal3_S2 K(625, 625, 0, 320, 240);
|
||||
boost::shared_ptr<Cal3_S2> K(new Cal3_S2(625, 625, 0, 320, 240));
|
||||
boost::shared_ptr<Graph> graph(new Graph());
|
||||
|
||||
graph->add(PoseConstraint(1,camera1));
|
||||
|
||||
StereoPoint2 z14(320,320.0-50, 240);
|
||||
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
|
||||
graph->add(StereoFactor(z14,sigma, 1, 1, K, 0.5));
|
||||
|
||||
// Create a configuration corresponding to the ground truth
|
||||
boost::shared_ptr<Values> values(new Values());
|
||||
values->insert(1, camera1); // add camera at z=6.25m looking towards origin
|
||||
|
||||
Point3 l1(0, 0, 0);
|
||||
values->insert(1, l1); // add point at origin;
|
||||
|
||||
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
|
||||
|
||||
typedef gtsam::NonlinearOptimizer<Graph,Values,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> Optimizer; // optimization engine for this domain
|
||||
|
||||
NonlinearOptimizationParameters parameters(1.0, 1e-3, 0,
|
||||
100, 1.0, 10, NonlinearOptimizationParameters::SILENT, NonlinearOptimizationParameters::BOUNDED);
|
||||
|
||||
Optimizer optimizer(graph, values, ordering, make_shared<NonlinearOptimizationParameters>(parameters));
|
||||
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
|
||||
|
||||
// Iterate once, and the config should not have changed
|
||||
Optimizer afterOneIteration = optimizer.iterate();
|
||||
DOUBLES_EQUAL(0.0, afterOneIteration.error(), 1e-9);
|
||||
|
||||
// Complete solution
|
||||
Optimizer final = optimizer.gaussNewton(1E-9, 1E-5);
|
||||
|
||||
DOUBLES_EQUAL(0.0, final.error(), 1e-6);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue