revived SmartProjectionPoseFactor. Compiles now, going to fix unit tests now
Luca
parent
24128f15fc
commit
391386a654
120
.cproject
120
.cproject
|
|
@ -5,13 +5,13 @@
|
|||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
|
||||
<externalSettings/>
|
||||
<extensions>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
@ -60,13 +60,13 @@
|
|||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing">
|
||||
<externalSettings/>
|
||||
<extensions>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
@ -116,13 +116,13 @@
|
|||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
|
||||
<externalSettings/>
|
||||
<extensions>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
@ -819,10 +819,18 @@
|
|||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="SmartStereoProjectionFactorExample.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="SmartProjectionFactorExample_kitti_nonbatch.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>SmartStereoProjectionFactorExample.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>SmartProjectionFactorExample_kitti_nonbatch.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="SmartProjectionFactorExample_kitti.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>SmartProjectionFactorExample_kitti.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -835,14 +843,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="SmartProjectionFactorExample.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>SmartProjectionFactorExample.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1035,30 +1035,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testCameraSet.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testCameraSet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testTriangulation.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testTriangulation.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPinholeSet.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testPinholeSet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1147,6 +1123,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSmartProjectionFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSmartProjectionFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testTSAMFactors.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -1546,10 +1530,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRegularImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testRegularImplicitSchurFactor.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testImplicitSchurFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1578,30 +1562,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSmartProjectionCameraFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartProjectionCameraFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testSmartFactorBase.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testSmartFactorBase.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testTriangulationFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testTriangulationFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j3</buildArguments>
|
||||
|
|
@ -2503,14 +2463,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="SFMExample_SmartFactorPCG.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>SFMExample_SmartFactorPCG.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -3103,22 +3055,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRegularHessianFactor.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testRegularHessianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRegularJacobianFactor.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>testRegularJacobianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testWrap.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
|
|||
|
|
@ -31,6 +31,16 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/// Linearization mode: what factor to linearize to
|
||||
enum LinearizationMode {
|
||||
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
|
||||
};
|
||||
|
||||
/// How to manage degeneracy
|
||||
enum DegeneracyMode {
|
||||
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
|
||||
};
|
||||
|
||||
/**
|
||||
* SmartProjectionFactor: triangulates point and keeps an estimate of it around.
|
||||
*/
|
||||
|
|
@ -39,16 +49,6 @@ class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
|
|||
|
||||
public:
|
||||
|
||||
/// Linearization mode: what factor to linearize to
|
||||
enum LinearizationMode {
|
||||
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
|
||||
};
|
||||
|
||||
/// How to manage degeneracy
|
||||
enum DegeneracyMode {
|
||||
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
|
||||
};
|
||||
|
||||
private:
|
||||
typedef SmartFactorBase<CAMERA> Base;
|
||||
typedef SmartProjectionFactor<CAMERA> This;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,160 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SmartProjectionPoseFactor.h
|
||||
* @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark
|
||||
* @author Luca Carlone
|
||||
* @author Chris Beall
|
||||
* @author Zsolt Kira
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/SmartProjectionFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
/**
|
||||
*
|
||||
* @addtogroup SLAM
|
||||
*
|
||||
* If you are using the factor, please cite:
|
||||
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
|
||||
* independent sets in factor graphs: a unifying perspective based on smart factors,
|
||||
* Int. Conf. on Robotics and Automation (ICRA), 2014.
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class CALIBRATION>
|
||||
class SmartProjectionPoseFactor: public SmartProjectionFactor<
|
||||
PinholePose<CALIBRATION> > {
|
||||
|
||||
private:
|
||||
typedef PinholePose<CALIBRATION> Camera;
|
||||
typedef SmartProjectionFactor<Camera> Base;
|
||||
typedef SmartProjectionPoseFactor<CALIBRATION> This;
|
||||
|
||||
protected:
|
||||
|
||||
boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
|
||||
std::vector<boost::shared_ptr<CALIBRATION> > sharedKs_; ///< shared pointer to calibration object (one for each camera)
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param rankTol tolerance used to check if point triangulation is degenerate
|
||||
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
|
||||
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
|
||||
* otherwise the factor is simply neglected (this functionality is deprecated)
|
||||
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
|
||||
* @param body_P_sensor is the transform from sensor to body frame (default identity)
|
||||
*/
|
||||
SmartProjectionPoseFactor(const double rankTol = 1,
|
||||
const double linThreshold = -1, const DegeneracyMode manageDegeneracy = IGNORE_DEGENERACY,
|
||||
const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1) :
|
||||
Base(linearizeTo, rankTol, manageDegeneracy, enableEPI,
|
||||
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), body_P_sensor_(body_P_sensor) {}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartProjectionPoseFactor() {}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||
DefaultKeyFormatter) const {
|
||||
std::cout << s << "SmartProjectionPoseFactor, z = \n ";
|
||||
if(body_P_sensor_)
|
||||
body_P_sensor_->print("body_P_sensor_:\n");
|
||||
Base::print("", keyFormatter);
|
||||
}
|
||||
|
||||
/// equals
|
||||
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
|
||||
const This *e = dynamic_cast<const This*>(&p);
|
||||
return e && Base::equals(p, tol);
|
||||
}
|
||||
|
||||
/**
|
||||
* Linearize to Gaussian Factor
|
||||
* @param values Values structure which must contain camera poses for this factor
|
||||
* @return
|
||||
*/
|
||||
virtual boost::shared_ptr<GaussianFactor> linearize(
|
||||
const Values& values) const {
|
||||
// depending on flag set on construction we may linearize to different linear factors
|
||||
// switch(linearizeTo_){
|
||||
// case JACOBIAN_SVD :
|
||||
// return this->createJacobianSVDFactor(Base::cameras(values), 0.0);
|
||||
// break;
|
||||
// case JACOBIAN_Q :
|
||||
// return this->createJacobianQFactor(Base::cameras(values), 0.0);
|
||||
// break;
|
||||
// default:
|
||||
return this->createHessianFactor(Base::cameras(values));
|
||||
// break;
|
||||
// }
|
||||
}
|
||||
|
||||
/**
|
||||
* error calculates the error of the factor.
|
||||
*/
|
||||
virtual double error(const Values& values) const {
|
||||
if (this->active(values)) {
|
||||
return this->totalReprojectionError(Base::cameras(values));
|
||||
} else { // else of active flag
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/** return calibration shared pointers */
|
||||
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
|
||||
return sharedKs_;
|
||||
}
|
||||
|
||||
Pose3 body_P_sensor() const{
|
||||
if(body_P_sensor_)
|
||||
return *body_P_sensor_;
|
||||
else
|
||||
return Pose3(); // if unspecified, the transformation is the identity
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
ar & BOOST_SERIALIZATION_NVP(sharedKs_);
|
||||
}
|
||||
|
||||
}; // end of class declaration
|
||||
|
||||
/// traits
|
||||
template<class CALIBRATION>
|
||||
struct traits<SmartProjectionPoseFactor<CALIBRATION> > : public Testable<
|
||||
SmartProjectionPoseFactor<CALIBRATION> > {
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
@ -17,6 +17,7 @@
|
|||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam/slam/SmartProjectionPoseFactor.h>
|
||||
#include <gtsam/slam/SmartProjectionFactor.h>
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
|
|
@ -66,7 +67,7 @@ typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
|||
// default Cal3_S2 poses
|
||||
namespace vanillaPose {
|
||||
typedef PinholePose<Cal3_S2> Camera;
|
||||
typedef SmartProjectionFactor<Camera> SmartFactor;
|
||||
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
|
||||
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
|
||||
Camera level_camera(level_pose, sharedK);
|
||||
Camera level_camera_right(pose_right, sharedK);
|
||||
|
|
@ -108,7 +109,7 @@ typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
|||
// Cal3Bundler poses
|
||||
namespace bundlerPose {
|
||||
typedef PinholePose<Cal3Bundler> Camera;
|
||||
typedef SmartProjectionFactor<Camera> SmartFactor;
|
||||
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
|
||||
static boost::shared_ptr<Cal3Bundler> sharedBundlerK(
|
||||
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
|
||||
Camera level_camera(level_pose, sharedBundlerK);
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ TEST( SmartProjectionPoseFactor, Constructor) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Constructor2) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor factor1(SmartFactor::HESSIAN, rankTol);
|
||||
SmartFactor factor1(gtsam::HESSIAN, rankTol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -72,7 +72,7 @@ TEST( SmartProjectionPoseFactor, Constructor3) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Constructor4) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor factor1(SmartFactor::HESSIAN, rankTol);
|
||||
SmartFactor factor1(gtsam::HESSIAN, rankTol);
|
||||
factor1.add(measurement1, x1, model);
|
||||
}
|
||||
|
||||
|
|
@ -495,15 +495,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD));
|
||||
new SmartFactor(gtsam::JACOBIAN_SVD));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD));
|
||||
new SmartFactor(gtsam::JACOBIAN_SVD));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD));
|
||||
new SmartFactor(gtsam::JACOBIAN_SVD));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -549,21 +549,18 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
|
||||
SmartFactor::IGNORE_DEGENERACY, false,
|
||||
excludeLandmarksFutherThanDist));
|
||||
new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
|
||||
SmartFactor::IGNORE_DEGENERACY, false,
|
||||
excludeLandmarksFutherThanDist));
|
||||
new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
|
||||
SmartFactor::IGNORE_DEGENERACY, false,
|
||||
excludeLandmarksFutherThanDist));
|
||||
new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -617,27 +614,23 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
|
||||
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
|
||||
dynamicOutlierRejectionThreshold));
|
||||
new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
|
||||
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
|
||||
dynamicOutlierRejectionThreshold));
|
||||
new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
|
||||
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
|
||||
dynamicOutlierRejectionThreshold));
|
||||
new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1,
|
||||
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist,
|
||||
dynamicOutlierRejectionThreshold));
|
||||
new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
smartFactor4->add(measurements_cam4, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -680,15 +673,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_Q));
|
||||
new SmartFactor(gtsam::JACOBIAN_Q));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_Q));
|
||||
new SmartFactor(gtsam::JACOBIAN_Q));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(SmartFactor::JACOBIAN_Q));
|
||||
new SmartFactor(gtsam::JACOBIAN_Q));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -799,15 +792,15 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
|
|||
double rankTol = 10;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol));
|
||||
new SmartFactor(gtsam::HESSIAN, rankTol));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol));
|
||||
new SmartFactor(gtsam::HESSIAN, rankTol));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol));
|
||||
new SmartFactor(gtsam::HESSIAN, rankTol));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
|
|
@ -880,13 +873,13 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
|
|||
|
||||
double rankTol = 50;
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol,
|
||||
SmartFactor::HANDLE_INFINITY));
|
||||
new SmartFactor(gtsam::HESSIAN, rankTol,
|
||||
gtsam::HANDLE_INFINITY));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol,
|
||||
SmartFactor::HANDLE_INFINITY));
|
||||
new SmartFactor(gtsam::HESSIAN, rankTol,
|
||||
gtsam::HANDLE_INFINITY));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -961,18 +954,18 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
|
|||
double rankTol = 10;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol,
|
||||
SmartFactor::ZERO_ON_DEGENERACY));
|
||||
new SmartFactor(gtsam::HESSIAN, rankTol,
|
||||
gtsam::ZERO_ON_DEGENERACY));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol,
|
||||
SmartFactor::ZERO_ON_DEGENERACY));
|
||||
new SmartFactor(gtsam::HESSIAN, rankTol,
|
||||
gtsam::ZERO_ON_DEGENERACY));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol,
|
||||
SmartFactor::ZERO_ON_DEGENERACY));
|
||||
new SmartFactor(gtsam::HESSIAN, rankTol,
|
||||
gtsam::ZERO_ON_DEGENERACY));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -1168,7 +1161,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
|
||||
using namespace bundlerPose;
|
||||
SmartFactor factor(SmartFactor::HESSIAN, rankTol);
|
||||
SmartFactor factor(gtsam::HESSIAN, rankTol);
|
||||
factor.add(measurement1, x1, model);
|
||||
}
|
||||
|
||||
|
|
@ -1262,18 +1255,18 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
|
|||
double rankTol = 10;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol,
|
||||
SmartFactor::ZERO_ON_DEGENERACY));
|
||||
new SmartFactor(rankTol, -1.0, gtsam::ZERO_ON_DEGENERACY,
|
||||
false, Pose3(), gtsam::HESSIAN));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol,
|
||||
SmartFactor::ZERO_ON_DEGENERACY));
|
||||
new SmartFactor(rankTol, -1, gtsam::ZERO_ON_DEGENERACY,
|
||||
false, Pose3(), gtsam::HESSIAN));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(SmartFactor::HESSIAN, rankTol,
|
||||
SmartFactor::ZERO_ON_DEGENERACY));
|
||||
new SmartFactor(rankTol, -1, gtsam::ZERO_ON_DEGENERACY,
|
||||
false, Pose3(), gtsam::HESSIAN));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
|
|||
Loading…
Reference in New Issue