Fixed Link errors ,etc for Multi-Disparity Factor
Natesh Srinivasan
parent
a1ef0c9c42
commit
a75df298d7
|
|
@ -6,5 +6,28 @@
|
|||
*/
|
||||
|
||||
|
||||
#include "MultiDisparityFactor.h"
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
//***************************************************************************
|
||||
|
||||
void MultiDisparityFactor::print(const string& s) const {
|
||||
cout << "Prior Factor on " << landmarkKey_ << "\n";
|
||||
cout << "Measured Disparities : \n " << disparities_ << "\n";
|
||||
this->noiseModel_->print(" Noise model: ");
|
||||
};
|
||||
|
||||
//***************************************************************************
|
||||
|
||||
Vector MultiDisparityFactor::evaluateError(const OrientedPlane3& plane,
|
||||
boost::optional<Matrix&> H) const {
|
||||
|
||||
return Vector_(3,1,1,1);
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,7 +16,11 @@
|
|||
* @brief A factor for modeling the disparity across multiple views
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/OrientedPlane3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -24,79 +28,36 @@ namespace gtsam {
|
|||
* Unary factor on measured disparity from multiple views as deterministic function of camera pose
|
||||
*/
|
||||
|
||||
class MutiDisparityFactor : public NoiseModelFactor1<OrientedPlane3> {
|
||||
class MultiDisparityFactor: public NoiseModelFactor1<OrientedPlane3> {
|
||||
|
||||
protected :
|
||||
Symbol landmarkSymbol_;
|
||||
OrientedPlane3 measured_p_;
|
||||
protected :
|
||||
|
||||
typedef NoiseModelFactor1<OrientedPlane3 > Base;
|
||||
Key landmarkKey_; // the key of the hidden plane in the world
|
||||
Vector disparities_; // measured disparities in a set of Superpixels \mathcal{V}
|
||||
|
||||
public:
|
||||
typedef NoiseModelFactor1<OrientedPlane3> Base;
|
||||
|
||||
/// Constructor
|
||||
MutiDisparityFactor ()
|
||||
{}
|
||||
public:
|
||||
|
||||
/// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
|
||||
MutiDisparityFactor (const Vector&z, const SharedGaussian& noiseModel,
|
||||
const Symbol& pose,
|
||||
const Symbol& landmark)
|
||||
: Base (noiseModel, landmark),
|
||||
poseSymbol_ (pose),
|
||||
landmarkSymbol_ (landmark),
|
||||
measured_coeffs_ (z)
|
||||
{
|
||||
measured_p_ = OrientedPlane3 (Sphere2 (z (0), z (1), z (2)), z (3));
|
||||
}
|
||||
// Constructor
|
||||
MultiDisparityFactor()
|
||||
{};
|
||||
|
||||
/// print
|
||||
void print(const std::string& s="PlaneFactor") const;
|
||||
/// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol
|
||||
MultiDisparityFactor (Key key, const Vector& disparities,
|
||||
const SharedIsotropic& noiseModel)
|
||||
: Base (noiseModel, key),
|
||||
landmarkKey_ (key),
|
||||
disparities_(disparities)
|
||||
{};
|
||||
|
||||
virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const
|
||||
{
|
||||
OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2);
|
||||
Vector error = predicted_plane.error (measured_p_);
|
||||
return (error);
|
||||
};
|
||||
};
|
||||
|
||||
class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> {
|
||||
|
||||
protected:
|
||||
OrientedPlane3 measured_p_; /// measured plane parameters
|
||||
Symbol landmarkSymbol_;
|
||||
|
||||
typedef NoiseModelFactor1<OrientedPlane3 > Base;
|
||||
|
||||
public:
|
||||
|
||||
typedef OrientedPlane3DirectionPrior This;
|
||||
/// Constructor
|
||||
OrientedPlane3DirectionPrior ()
|
||||
{}
|
||||
|
||||
/// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol
|
||||
OrientedPlane3DirectionPrior (const Symbol& landmark, const Vector&z,
|
||||
const SharedGaussian& noiseModel)
|
||||
: Base (noiseModel, landmark),
|
||||
landmarkSymbol_ (landmark)
|
||||
{
|
||||
measured_p_ = OrientedPlane3 (Sphere2 (z (0), z (1), z (2)), z (3));
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s) const;
|
||||
|
||||
/** equals */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||
|
||||
virtual Vector evaluateError(const OrientedPlane3& plane,
|
||||
boost::optional<Matrix&> H = boost::none) const;
|
||||
};
|
||||
/// print
|
||||
void print(const std::string& s="Multi-View DisaprityFactor") const;
|
||||
|
||||
virtual Vector evaluateError(const OrientedPlane3& plane,
|
||||
boost::optional<Matrix&> H1 = boost::none) const;
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,58 @@
|
|||
/*
|
||||
* testMultiDisparityFactor.cpp
|
||||
*
|
||||
* Created on: Jan 31, 2014
|
||||
* Author: nsrinivasan7
|
||||
* @brief: Unittest for MultidisparityFactor
|
||||
*/
|
||||
|
||||
|
||||
#include <gtsam/geometry/Sphere2.h>
|
||||
#include <gtsam/geometry/OrientedPlane3.h>
|
||||
#include <gtsam/slam/OrientedPlane3Factor.h>
|
||||
#include <gtsam/slam/MultiDisparityFactor.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
|
||||
using namespace boost::assign;
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
|
||||
|
||||
TEST(MutliDisparityFactor,error)
|
||||
{
|
||||
|
||||
Key key(1);
|
||||
Vector disparities = Vector_(2, 1.0, 1.0); // matlab generated values
|
||||
|
||||
SharedIsotropic model = gtsam::noiseModel::Isotropic::Sigma(disparities.rows(), 0.25, true);
|
||||
|
||||
cout << "Vector # main :" << disparities << endl;
|
||||
|
||||
MultiDisparityFactor factor(key, disparities, model);
|
||||
factor.print("Multi-disparity Factor");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
srand(time(NULL));
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue