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

Fix check.slam

release/4.3a0
Frank Dellaert 2022-02-16 09:30:59 -05:00
parent 1aedbf76a2
commit 8e33e96efa
6 changed files with 235 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
gtsam/slam/tests/PinholeFactor.h Normal file
View File

@ -0,0 +1,52 @@
/* ----------------------------------------------------------------------------
* 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 PinholeFactor.h
* @brief helper class for tests
* @author Frank Dellaert
* @date February 2022
*/
#pragma once
namespace gtsam {
template <typename T>
struct traits;
}
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/SmartFactorBase.h>
namespace gtsam {
class PinholeFactor : public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor() {}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10)
: Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
double error(const Values& values) const override { return 0.0; }
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
}
};
/// traits
template <>
struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
} // namespace gtsam

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

@ -29,6 +29,7 @@
using namespace std;
using namespace gtsam;
namespace {
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
@ -48,23 +49,24 @@ static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static double fov = 60; // degrees
static size_t w = 640, h = 480;
}
/* ************************************************************************* */
// default Cal3_S2 cameras
namespace vanilla {
typedef PinholeCamera<Cal3_S2> Camera;
typedef SmartProjectionFactor<Camera> SmartFactor;
static Cal3_S2 K(fov, w, h);
static Cal3_S2 K2(1500, 1200, 0, w, h);
Camera level_camera(level_pose, K2);
Camera level_camera_right(pose_right, K2);
Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1);
Camera cam1(level_pose, K2);
Camera cam2(pose_right, K2);
Camera cam3(pose_above, K2);
static const Cal3_S2 K(fov, w, h);
static const Cal3_S2 K2(1500, 1200, 0, w, h);
static const Camera level_camera(level_pose, K2);
static const Camera level_camera_right(pose_right, K2);
static const Point2 level_uv = level_camera.project(landmark1);
static const Point2 level_uv_right = level_camera_right.project(landmark1);
static const Camera cam1(level_pose, K2);
static const Camera cam2(pose_right, K2);
static const Camera cam3(pose_above, K2);
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
SmartProjectionParams params;
static const SmartProjectionParams params;
} // namespace vanilla
/* ************************************************************************* */
@ -74,12 +76,12 @@ typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
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);
Camera cam1(level_pose, sharedK);
Camera cam2(pose_right, sharedK);
Camera cam3(pose_above, sharedK);
static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
static const Camera level_camera(level_pose, sharedK);
static const Camera level_camera_right(pose_right, sharedK);
static const Camera cam1(level_pose, sharedK);
static const Camera cam2(pose_right, sharedK);
static const Camera cam3(pose_above, sharedK);
} // namespace vanillaPose
/* ************************************************************************* */
@ -89,12 +91,12 @@ typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
Camera level_camera(level_pose, sharedK2);
Camera level_camera_right(pose_right, sharedK2);
Camera cam1(level_pose, sharedK2);
Camera cam2(pose_right, sharedK2);
Camera cam3(pose_above, sharedK2);
static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
static const Camera level_camera(level_pose, sharedK2);
static const Camera level_camera_right(pose_right, sharedK2);
static const Camera cam1(level_pose, sharedK2);
static const Camera cam2(pose_right, sharedK2);
static const Camera cam3(pose_above, sharedK2);
} // namespace vanillaPose2
/* *************************************************************************/
@ -103,15 +105,15 @@ namespace bundler {
typedef PinholeCamera<Cal3Bundler> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionFactor<Camera> SmartFactor;
static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
Camera level_camera(level_pose, K);
Camera level_camera_right(pose_right, K);
Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1);
Pose3 pose1 = level_pose;
Camera cam1(level_pose, K);
Camera cam2(pose_right, K);
Camera cam3(pose_above, K);
static const Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
static const Camera level_camera(level_pose, K);
static const Camera level_camera_right(pose_right, K);
static const Point2 level_uv = level_camera.project(landmark1);
static const Point2 level_uv_right = level_camera_right.project(landmark1);
static const Pose3 pose1 = level_pose;
static const Camera cam1(level_pose, K);
static const Camera cam2(pose_right, K);
static const Camera cam3(pose_above, K);
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
} // namespace bundler
@ -122,14 +124,14 @@ typedef PinholePose<Cal3Bundler> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static boost::shared_ptr<Cal3Bundler> sharedBundlerK(new Cal3Bundler(500, 1e-3,
static const boost::shared_ptr<Cal3Bundler> sharedBundlerK(new Cal3Bundler(500, 1e-3,
1e-3, 1000,
2000));
Camera level_camera(level_pose, sharedBundlerK);
Camera level_camera_right(pose_right, sharedBundlerK);
Camera cam1(level_pose, sharedBundlerK);
Camera cam2(pose_right, sharedBundlerK);
Camera cam3(pose_above, sharedBundlerK);
static const Camera level_camera(level_pose, sharedBundlerK);
static const Camera level_camera_right(pose_right, sharedBundlerK);
static const Camera cam1(level_pose, sharedBundlerK);
static const Camera cam2(pose_right, sharedBundlerK);
static const Camera cam3(pose_above, sharedBundlerK);
} // namespace bundlerPose
/* ************************************************************************* */
@ -138,12 +140,12 @@ namespace sphericalCamera {
typedef SphericalCamera Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionRigFactor<Camera> SmartFactorP;
static EmptyCal::shared_ptr emptyK(new EmptyCal());
Camera level_camera(level_pose);
Camera level_camera_right(pose_right);
Camera cam1(level_pose);
Camera cam2(pose_right);
Camera cam3(pose_above);
static const EmptyCal::shared_ptr emptyK(new EmptyCal());
static const Camera level_camera(level_pose);
static const Camera level_camera_right(pose_right);
static const Camera cam1(level_pose);
static const Camera cam2(pose_right);
static const Camera cam3(pose_above);
} // namespace sphericalCamera
/* *************************************************************************/

104
gtsam/slam/tests/testSerializationSlam.cpp Normal file
View File

@ -0,0 +1,104 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationSlam.cpp
* @brief all serialization tests in this directory
* @author Frank Dellaert
* @date February 2022
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/SmartProjectionFactor.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
#include "smartFactorScenarios.h"
#include "PinholeFactor.h"
namespace {
static const double rankTol = 1.0;
static const double sigma = 0.1;
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
} // namespace
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(SharedDiagonal, "gtsam_SharedDiagonal")
/* ************************************************************************* */
TEST(SmartFactorBase, serialize) {
using namespace serializationTestHelpers;
PinholeFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
TEST(SerializationSlam, SmartProjectionFactor) {
using namespace vanilla;
using namespace serializationTestHelpers;
SmartFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
TEST(SerializationSlam, SmartProjectionPoseFactor) {
using namespace vanillaPose;
using namespace serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor(model, sharedK, params);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
TEST(SerializationSlam, SmartProjectionPoseFactor2) {
using namespace vanillaPose;
using namespace serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
Pose3 bts;
SmartFactor factor(model, sharedK, bts, params);
// insert some measurments
KeyVector key_view;
Point2Vector meas_view;
key_view.push_back(Symbol('x', 1));
meas_view.push_back(Point2(10, 10));
factor.add(meas_view, key_view);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -16,47 +16,29 @@
* @date Feb 2015
*/
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/slam/SmartFactorBase.h>
using namespace std;
#include "PinholeFactor.h"
namespace {
using namespace gtsam;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
} // namespace
/* ************************************************************************* */
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
using namespace std;
namespace gtsam {
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor() {}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10)
: Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
double error(const Values& values) const override { return 0.0; }
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
}
};
/// traits
template<>
struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
}
TEST(SmartFactorBase, Pinhole) {
PinholeFactor f= PinholeFactor(unit2);
f.add(Point2(0,0), 1);
f.add(Point2(0,0), 2);
PinholeFactor f = PinholeFactor(unit2);
f.add(Point2(0, 0), 1);
f.add(Point2(0, 0), 2);
EXPECT_LONGS_EQUAL(2 * 2, f.dim());
}
@ -71,7 +53,7 @@ TEST(SmartFactorBase, PinholeWithSensor) {
// Camera coordinates in world frame.
Pose3 wTc = world_P_body * body_P_sensor;
cameras.push_back(PinholeCamera<Cal3Bundler>(wTc));
// Simple point to project slightly off image center
Point3 p(0, 0, 10);
Point2 measurement = cameras[0].project(p);
@ -81,9 +63,10 @@ TEST(SmartFactorBase, PinholeWithSensor) {
Matrix E;
Vector error = f.unwhitenedError<Point3>(cameras, p, Fs, E);
Vector expectedError = Vector::Zero(2);
Vector expectedError = Vector::Zero(2);
Matrix29 expectedFs;
expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0;
expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1,
0, 0, 0, 0;
Matrix23 expectedE;
expectedE << 0.1, 0, 0.01, 0, 0.1, 0;
@ -94,20 +77,13 @@ TEST(SmartFactorBase, PinholeWithSensor) {
EXPECT(assert_equal(expectedE, E));
}
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
namespace gtsam {
class StereoFactor: public SmartFactorBase<StereoCamera> {
public:
class StereoFactor : public SmartFactorBase<StereoCamera> {
public:
typedef SmartFactorBase<StereoCamera> Base;
StereoFactor() {}
StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
double error(const Values& values) const override {
return 0.0;
}
StereoFactor(const SharedNoiseModel& sharedNoiseModel)
: Base(sharedNoiseModel) {}
double error(const Values& values) const override { return 0.0; }
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
@ -115,9 +91,8 @@ public:
};
/// traits
template<>
template <>
struct traits<StereoFactor> : public Testable<StereoFactor> {};
}
TEST(SmartFactorBase, Stereo) {
StereoFactor f(unit3);
@ -125,24 +100,7 @@ TEST(SmartFactorBase, Stereo) {
f.add(StereoPoint2(), 2);
EXPECT_LONGS_EQUAL(2 * 3, f.dim());
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
TEST(SmartFactorBase, serialize) {
using namespace gtsam::serializationTestHelpers;
PinholeFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
} // namespace gtsam
/* ************************************************************************* */
int main() {
@ -150,4 +108,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

32
gtsam/slam/tests/testSmartProjectionFactor.cpp
View File

@ -22,18 +22,19 @@
#include "smartFactorScenarios.h"
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
using namespace boost::assign;
namespace {
static const bool isDebugTest = false;
static const Symbol l1('l', 1), l2('l', 2), l3('l', 3);
static const Key c1 = 1, c2 = 2, c3 = 3;
static const Point2 measurement1(323.0, 240.0);
static const double rankTol = 1.0;
}
template<class CALIBRATION>
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
@ -70,8 +71,9 @@ TEST(SmartProjectionFactor, Constructor) {
/* ************************************************************************* */
TEST(SmartProjectionFactor, Constructor2) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(unit2, params);
auto myParams = params;
myParams.setRankTolerance(rankTol);
SmartFactor factor1(unit2, myParams);
}
/* ************************************************************************* */
@ -84,8 +86,9 @@ TEST(SmartProjectionFactor, Constructor3) {
/* ************************************************************************* */
TEST(SmartProjectionFactor, Constructor4) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(unit2, params);
auto myParams = params;
myParams.setRankTolerance(rankTol);
SmartFactor factor1(unit2, myParams);
factor1.add(measurement1, c1);
}
@ -810,25 +813,6 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) {
EXPECT(assert_equal(yActual, yExpected, 1e-7));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
TEST(SmartProjectionFactor, serialize) {
using namespace vanilla;
using namespace gtsam::serializationTestHelpers;
SmartFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -24,7 +24,6 @@
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
@ -32,6 +31,7 @@
using namespace boost::assign;
using namespace std::placeholders;
namespace {
static const double rankTol = 1.0;
// Create a noise model for the pixel error
static const double sigma = 0.1;
@ -51,6 +51,7 @@ static Point2 measurement1(323.0, 240.0);
LevenbergMarquardtParams lmParams;
// Make more verbose like so (in tests):
// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY;
}
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor) {
@ -1332,47 +1333,6 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
values.at<Pose3>(x3)));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
TEST(SmartProjectionPoseFactor, serialize) {
using namespace vanillaPose;
using namespace gtsam::serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor(model, sharedK, params);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
TEST(SmartProjectionPoseFactor, serialize2) {
using namespace vanillaPose;
using namespace gtsam::serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
Pose3 bts;
SmartFactor factor(model, sharedK, bts, params);
// insert some measurments
KeyVector key_view;
Point2Vector meas_view;
key_view.push_back(Symbol('x', 1));
meas_view.push_back(Point2(10, 10));
factor.add(meas_view, key_view);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;