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

revived SmartProjectionPoseFactor. Compiles now, going to fix unit tests now

release/4.3a0
Luca 2015-06-18 16:00:54 -04:00
parent 24128f15fc
commit 391386a654
5 changed files with 243 additions and 153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
.cproject
View File

@ -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"> <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/> <externalSettings/>
<extensions> <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.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.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.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.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.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> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <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"> <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/> <externalSettings/>
<extensions> <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.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.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.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.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.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> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <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"> <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/> <externalSettings/>
<extensions> <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.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.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.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.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.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> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -819,10 +819,18 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>SmartStereoProjectionFactorExample.run</buildTarget> <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> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -835,14 +843,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1035,30 +1035,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1147,6 +1123,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testTSAMFactors.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1546,10 +1530,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testRegularImplicitSchurFactor.run</buildTarget> <buildTarget>testImplicitSchurFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -1578,30 +1562,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j3</buildArguments> <buildArguments>-j3</buildArguments>
@ -2503,14 +2463,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -3103,22 +3055,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testWrap.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>

20
gtsam/slam/SmartProjectionFactor.h
View File

@ -31,6 +31,16 @@
namespace gtsam { 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. * SmartProjectionFactor: triangulates point and keeps an estimate of it around.
*/ */
@ -39,16 +49,6 @@ class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
public: 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: private:
typedef SmartFactorBase<CAMERA> Base; typedef SmartFactorBase<CAMERA> Base;
typedef SmartProjectionFactor<CAMERA> This; typedef SmartProjectionFactor<CAMERA> This;

160
gtsam/slam/SmartProjectionPoseFactor.h Normal file
View File

@ -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

5
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -17,6 +17,7 @@
*/ */
#pragma once #pragma once
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/slam/SmartProjectionFactor.h> #include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
@ -66,7 +67,7 @@ typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
// default Cal3_S2 poses // default Cal3_S2 poses
namespace vanillaPose { namespace vanillaPose {
typedef PinholePose<Cal3_S2> Camera; 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)); static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
Camera level_camera(level_pose, sharedK); Camera level_camera(level_pose, sharedK);
Camera level_camera_right(pose_right, sharedK); Camera level_camera_right(pose_right, sharedK);
@ -108,7 +109,7 @@ typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
// Cal3Bundler poses // Cal3Bundler poses
namespace bundlerPose { namespace bundlerPose {
typedef PinholePose<Cal3Bundler> Camera; typedef PinholePose<Cal3Bundler> Camera;
typedef SmartProjectionFactor<Camera> SmartFactor; typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
static boost::shared_ptr<Cal3Bundler> sharedBundlerK( static boost::shared_ptr<Cal3Bundler> sharedBundlerK(
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
Camera level_camera(level_pose, sharedBundlerK); Camera level_camera(level_pose, sharedBundlerK);

91
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -59,7 +59,7 @@ TEST( SmartProjectionPoseFactor, Constructor) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor2) { TEST( SmartProjectionPoseFactor, Constructor2) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactor factor1(SmartFactor::HESSIAN, rankTol); SmartFactor factor1(gtsam::HESSIAN, rankTol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -72,7 +72,7 @@ TEST( SmartProjectionPoseFactor, Constructor3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor4) { TEST( SmartProjectionPoseFactor, Constructor4) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactor factor1(SmartFactor::HESSIAN, rankTol); SmartFactor factor1(gtsam::HESSIAN, rankTol);
factor1.add(measurement1, x1, model); factor1.add(measurement1, x1, model);
} }
@ -495,15 +495,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::JACOBIAN_SVD)); new SmartFactor(gtsam::JACOBIAN_SVD));
smartFactor1->add(measurements_cam1, views, model); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::JACOBIAN_SVD)); new SmartFactor(gtsam::JACOBIAN_SVD));
smartFactor2->add(measurements_cam2, views, model); smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::JACOBIAN_SVD)); new SmartFactor(gtsam::JACOBIAN_SVD));
smartFactor3->add(measurements_cam3, views, model); smartFactor3->add(measurements_cam3, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -549,21 +549,18 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
SmartFactor::IGNORE_DEGENERACY, false, gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist));
excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
SmartFactor::IGNORE_DEGENERACY, false, gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist));
excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model); smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
SmartFactor::IGNORE_DEGENERACY, false, gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist));
excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model); smartFactor3->add(measurements_cam3, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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 measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model); smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model); smartFactor3->add(measurements_cam3, views, model);
SmartFactor::shared_ptr smartFactor4( SmartFactor::shared_ptr smartFactor4(
new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, new SmartFactor(1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model); smartFactor4->add(measurements_cam4, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -680,15 +673,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::JACOBIAN_Q)); new SmartFactor(gtsam::JACOBIAN_Q));
smartFactor1->add(measurements_cam1, views, model); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::JACOBIAN_Q)); new SmartFactor(gtsam::JACOBIAN_Q));
smartFactor2->add(measurements_cam2, views, model); smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::JACOBIAN_Q)); new SmartFactor(gtsam::JACOBIAN_Q));
smartFactor3->add(measurements_cam3, views, model); smartFactor3->add(measurements_cam3, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -799,15 +792,15 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
double rankTol = 10; double rankTol = 10;
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::HESSIAN, rankTol)); new SmartFactor(gtsam::HESSIAN, rankTol));
smartFactor1->add(measurements_cam1, views, model); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::HESSIAN, rankTol)); new SmartFactor(gtsam::HESSIAN, rankTol));
smartFactor2->add(measurements_cam2, views, model); smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::HESSIAN, rankTol)); new SmartFactor(gtsam::HESSIAN, rankTol));
smartFactor3->add(measurements_cam3, views, model); smartFactor3->add(measurements_cam3, views, model);
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -880,13 +873,13 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
double rankTol = 50; double rankTol = 50;
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::HESSIAN, rankTol, new SmartFactor(gtsam::HESSIAN, rankTol,
SmartFactor::HANDLE_INFINITY)); gtsam::HANDLE_INFINITY));
smartFactor1->add(measurements_cam1, views, model); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::HESSIAN, rankTol, new SmartFactor(gtsam::HESSIAN, rankTol,
SmartFactor::HANDLE_INFINITY)); gtsam::HANDLE_INFINITY));
smartFactor2->add(measurements_cam2, views, model); smartFactor2->add(measurements_cam2, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -961,18 +954,18 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
double rankTol = 10; double rankTol = 10;
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::HESSIAN, rankTol, new SmartFactor(gtsam::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY)); gtsam::ZERO_ON_DEGENERACY));
smartFactor1->add(measurements_cam1, views, model); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::HESSIAN, rankTol, new SmartFactor(gtsam::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY)); gtsam::ZERO_ON_DEGENERACY));
smartFactor2->add(measurements_cam2, views, model); smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::HESSIAN, rankTol, new SmartFactor(gtsam::HESSIAN, rankTol,
SmartFactor::ZERO_ON_DEGENERACY)); gtsam::ZERO_ON_DEGENERACY));
smartFactor3->add(measurements_cam3, views, model); smartFactor3->add(measurements_cam3, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1168,7 +1161,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
using namespace bundlerPose; using namespace bundlerPose;
SmartFactor factor(SmartFactor::HESSIAN, rankTol); SmartFactor factor(gtsam::HESSIAN, rankTol);
factor.add(measurement1, x1, model); factor.add(measurement1, x1, model);
} }
@ -1262,18 +1255,18 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
double rankTol = 10; double rankTol = 10;
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(SmartFactor::HESSIAN, rankTol, new SmartFactor(rankTol, -1.0, gtsam::ZERO_ON_DEGENERACY,
SmartFactor::ZERO_ON_DEGENERACY)); false, Pose3(), gtsam::HESSIAN));
smartFactor1->add(measurements_cam1, views, model); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(SmartFactor::HESSIAN, rankTol, new SmartFactor(rankTol, -1, gtsam::ZERO_ON_DEGENERACY,
SmartFactor::ZERO_ON_DEGENERACY)); false, Pose3(), gtsam::HESSIAN));
smartFactor2->add(measurements_cam2, views, model); smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(SmartFactor::HESSIAN, rankTol, new SmartFactor(rankTol, -1, gtsam::ZERO_ON_DEGENERACY,
SmartFactor::ZERO_ON_DEGENERACY)); false, Pose3(), gtsam::HESSIAN));
smartFactor3->add(measurements_cam3, views, model); smartFactor3->add(measurements_cam3, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);