testing mode: step 1: need to figure out the manifold stuff
lcarlone
parent
1f55e06a58
commit
e5677e3805
|
|
@ -121,6 +121,13 @@ public:
|
|||
return _project(pw, Dpose, Dpoint, Dcal);
|
||||
}
|
||||
|
||||
/// project a 3D point from world coordinates into the image
|
||||
Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none,
|
||||
OptionalJacobian<2, DimK> Dcal = boost::none) const {
|
||||
return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
|
||||
}
|
||||
|
||||
/// project a point at infinity from world coordinates into the image
|
||||
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 2> Dpoint = boost::none,
|
||||
|
|
|
|||
|
|
@ -65,11 +65,25 @@ Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const {
|
|||
return pose().transformFrom(depth * pu);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Unit3 SphericalCamera::backprojectPointAtInfinity(const Unit3& p) const {
|
||||
return pose().rotation().rotate(p);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Unit3 SphericalCamera::project(const Point3& point,
|
||||
OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
|
||||
return project2(point, Dcamera, Dpoint);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector2 SphericalCamera::reprojectionError(const Point3& point, const Unit3& measured,
|
||||
OptionalJacobian<2, 6> Dpose,
|
||||
OptionalJacobian<2, 3> Dpoint) const {
|
||||
// onmanifold version of: camera.project(point) - zi
|
||||
std::cout << "SphericalCam:reprojectionError fix jacobians " << std::endl;
|
||||
return measured.localCoordinates( project2(point, Dpose, Dpoint) );
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,6 +29,19 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class GTSAM_EXPORT EmptyCal {
|
||||
protected:
|
||||
double d_ = 0;
|
||||
public:
|
||||
///< shared pointer to calibration object
|
||||
EmptyCal()
|
||||
: d_(0) {
|
||||
}
|
||||
/// Default destructor
|
||||
virtual ~EmptyCal() = default;
|
||||
using shared_ptr = boost::shared_ptr<EmptyCal>;
|
||||
};
|
||||
|
||||
/**
|
||||
* A spherical camera class that has a Pose3 and measures bearing vectors
|
||||
* @addtogroup geometry
|
||||
|
|
@ -36,61 +49,69 @@ namespace gtsam {
|
|||
*/
|
||||
class GTSAM_EXPORT SphericalCamera {
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
enum {
|
||||
dimension = 6
|
||||
};
|
||||
|
||||
typedef Point2 Measurement;
|
||||
typedef Point2Vector MeasurementVector;
|
||||
typedef Unit3 Measurement;
|
||||
typedef std::vector<Unit3> MeasurementVector;
|
||||
typedef EmptyCal CalibrationType;
|
||||
|
||||
private:
|
||||
private:
|
||||
|
||||
Pose3 pose_; ///< 3D pose of camera
|
||||
Pose3 pose_; ///< 3D pose of camera
|
||||
|
||||
protected:
|
||||
protected:
|
||||
|
||||
/// @name Derivatives
|
||||
/// @{
|
||||
EmptyCal::shared_ptr emptyCal_;
|
||||
|
||||
// /**
|
||||
// * Calculate Jacobian with respect to pose
|
||||
// * @param pn projection in normalized coordinates
|
||||
// * @param d disparity (inverse depth)
|
||||
// */
|
||||
// static Matrix26 Dpose(const Point2& pn, double d);
|
||||
//
|
||||
// /**
|
||||
// * Calculate Jacobian with respect to point
|
||||
// * @param pn projection in normalized coordinates
|
||||
// * @param d disparity (inverse depth)
|
||||
// * @param Rt transposed rotation matrix
|
||||
// */
|
||||
// static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
|
||||
//
|
||||
// /// @}
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/// @}
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
SphericalCamera() {}
|
||||
SphericalCamera()
|
||||
: pose_(Pose3::identity()),
|
||||
emptyCal_(boost::make_shared<EmptyCal>()) {
|
||||
}
|
||||
|
||||
/// Constructor with pose
|
||||
explicit SphericalCamera(const Pose3& pose) : pose_(pose) {}
|
||||
explicit SphericalCamera(const Pose3& pose)
|
||||
: pose_(pose),
|
||||
emptyCal_(boost::make_shared<EmptyCal>()) {
|
||||
}
|
||||
|
||||
/// Constructor with empty intrinsics (needed for smart factors)
|
||||
explicit SphericalCamera(const Pose3& pose,
|
||||
const boost::shared_ptr<EmptyCal>& cal)
|
||||
: pose_(pose),
|
||||
emptyCal_(boost::make_shared<EmptyCal>()) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
|
||||
explicit SphericalCamera(const Vector& v)
|
||||
: pose_(Pose3::Expmap(v)) {
|
||||
}
|
||||
|
||||
/// Default destructor
|
||||
virtual ~SphericalCamera() = default;
|
||||
|
||||
/// return shared pointer to calibration
|
||||
const boost::shared_ptr<EmptyCal>& sharedCalibration() const {
|
||||
return emptyCal_;
|
||||
}
|
||||
|
||||
/// return calibration
|
||||
const EmptyCal& calibration() const {
|
||||
return *emptyCal_;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
@ -120,8 +141,8 @@ public:
|
|||
return pose_.translation();
|
||||
}
|
||||
|
||||
// /// return pose, with derivative
|
||||
// const Pose3& getPose(OptionalJacobian<6, 6> H) const;
|
||||
// /// return pose, with derivative
|
||||
// const Pose3& getPose(OptionalJacobian<6, 6> H) const;
|
||||
|
||||
/// @}
|
||||
/// @name Transformations and measurement functions
|
||||
|
|
@ -135,19 +156,30 @@ public:
|
|||
* @param point 3D point in world coordinates
|
||||
* @return the intrinsic coordinates of the projected point
|
||||
*/
|
||||
Unit3 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
|
||||
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
|
||||
Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none) const;
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
|
||||
Point3 backproject(const Unit3& p, const double depth) const;
|
||||
|
||||
/// backproject point at infinity
|
||||
Unit3 backprojectPointAtInfinity(const Unit3& p) const;
|
||||
|
||||
/** Project point into the image
|
||||
* (note: there is no CheiralityException for a spherical camera)
|
||||
* @param point 3D point in world coordinates
|
||||
* @return the intrinsic coordinates of the projected point
|
||||
*/
|
||||
Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose =
|
||||
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
|
||||
Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none) const;
|
||||
|
||||
/** Compute reprojection error for a given 3D point in world coordinates
|
||||
* @param point 3D point in world coordinates
|
||||
* @return the tangent space error between the projection and the measurement
|
||||
*/
|
||||
Vector2 reprojectionError(const Point3& point, const Unit3& measured,
|
||||
OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none) const;
|
||||
/// @}
|
||||
|
||||
/// move a cameras according to d
|
||||
|
|
@ -162,11 +194,10 @@ public:
|
|||
|
||||
/// for Canonical
|
||||
static SphericalCamera identity() {
|
||||
return SphericalCamera(Pose3::identity()); // assumes that the default constructor is valid
|
||||
return SphericalCamera(Pose3::identity()); // assumes that the default constructor is valid
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
|
|
@ -177,4 +208,12 @@ private:
|
|||
};
|
||||
// end of class SphericalCamera
|
||||
|
||||
template<>
|
||||
struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -474,8 +474,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
|
|||
#endif
|
||||
// Check reprojection error
|
||||
if (params.dynamicOutlierRejectionThreshold > 0) {
|
||||
const Point2& zi = measured.at(i);
|
||||
Point2 reprojectionError(camera.project(point) - zi);
|
||||
const typename CAMERA::Measurement& zi = measured.at(i);
|
||||
Point2 reprojectionError = camera.reprojectionError(point, zi);
|
||||
maxReprojError = std::max(maxReprojError, reprojectionError.norm());
|
||||
}
|
||||
i += 1;
|
||||
|
|
|
|||
|
|
@ -53,6 +53,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
|
|||
typedef SmartProjectionFactor<CAMERA> Base;
|
||||
typedef SmartProjectionFactorP<CAMERA> This;
|
||||
typedef typename CAMERA::CalibrationType CALIBRATION;
|
||||
typedef typename CAMERA::Measurement MEASUREMENT;
|
||||
typedef typename CAMERA::MeasurementVector MEASUREMENTS;
|
||||
|
||||
static const int DimPose = 6; ///< Pose3 dimension
|
||||
static const int ZDim = 2; ///< Measurement dimension (Point2)
|
||||
|
|
@ -108,7 +110,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
|
|||
* @param K (fixed) camera intrinsic calibration
|
||||
* @param body_P_sensor (fixed) camera extrinsic calibration
|
||||
*/
|
||||
void add(const Point2& measured, const Key& poseKey,
|
||||
void add(const MEASUREMENT& measured, const Key& poseKey,
|
||||
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor =
|
||||
Pose3::identity()) {
|
||||
// store measurement and key
|
||||
|
|
@ -133,7 +135,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
|
|||
* @param Ks vector of (fixed) intrinsic calibration objects
|
||||
* @param body_P_sensors vector of (fixed) extrinsic calibration objects
|
||||
*/
|
||||
void add(const Point2Vector& measurements, const std::vector<Key>& poseKeys,
|
||||
void add(const MEASUREMENTS& measurements, const std::vector<Key>& poseKeys,
|
||||
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
|
||||
const std::vector<Pose3> body_P_sensors = std::vector<Pose3>()) {
|
||||
assert(poseKeys.size() == measurements.size());
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@
|
|||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/SphericalCamera.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -123,6 +124,19 @@ Camera cam1(level_pose, sharedBundlerK);
|
|||
Camera cam2(pose_right, sharedBundlerK);
|
||||
Camera cam3(pose_above, sharedBundlerK);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// sphericalCamera
|
||||
namespace sphericalCamera {
|
||||
typedef SphericalCamera Camera;
|
||||
typedef SmartProjectionFactorP<Camera> SmartFactorP;
|
||||
static EmptyCal::shared_ptr emptyK;
|
||||
Camera level_camera(level_pose);
|
||||
Camera level_camera_right(pose_right);
|
||||
Camera cam1(level_pose);
|
||||
Camera cam2(pose_right);
|
||||
Camera cam3(pose_above);
|
||||
}
|
||||
/* *************************************************************************/
|
||||
|
||||
template<class CAMERA>
|
||||
|
|
@ -137,9 +151,9 @@ CAMERA perturbCameraPose(const CAMERA& camera) {
|
|||
template<class CAMERA>
|
||||
void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2,
|
||||
const CAMERA& cam3, Point3 landmark, typename CAMERA::MeasurementVector& measurements_cam) {
|
||||
Point2 cam1_uv1 = cam1.project(landmark);
|
||||
Point2 cam2_uv1 = cam2.project(landmark);
|
||||
Point2 cam3_uv1 = cam3.project(landmark);
|
||||
typename CAMERA::Measurement cam1_uv1 = cam1.project(landmark);
|
||||
typename CAMERA::Measurement cam2_uv1 = cam2.project(landmark);
|
||||
typename CAMERA::Measurement cam3_uv1 = cam3.project(landmark);
|
||||
measurements_cam.push_back(cam1_uv1);
|
||||
measurements_cam.push_back(cam2_uv1);
|
||||
measurements_cam.push_back(cam3_uv1);
|
||||
|
|
|
|||
|
|
@ -1086,6 +1086,133 @@ TEST( SmartProjectionFactorP, timing ) {
|
|||
}
|
||||
#endif
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) {
|
||||
|
||||
using namespace sphericalCamera;
|
||||
Camera::MeasurementVector measurements_lmk1, measurements_lmk2, measurements_lmk3;
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, measurements_lmk1);
|
||||
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, measurements_lmk2);
|
||||
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, measurements_lmk3);
|
||||
|
||||
// create inputs
|
||||
std::vector<Key> keys;
|
||||
keys.push_back(x1);
|
||||
keys.push_back(x2);
|
||||
keys.push_back(x3);
|
||||
|
||||
std::vector<EmptyCal::shared_ptr> emptyKs;
|
||||
emptyKs.push_back(emptyK);
|
||||
emptyKs.push_back(emptyK);
|
||||
emptyKs.push_back(emptyK);
|
||||
|
||||
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model));
|
||||
smartFactor1->add(measurements_lmk1, keys, emptyKs);
|
||||
|
||||
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model));
|
||||
// smartFactor2->add(measurements_lmk2, keys, sharedKs);
|
||||
//
|
||||
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model));
|
||||
// smartFactor3->add(measurements_lmk3, keys, sharedKs);
|
||||
//
|
||||
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
//
|
||||
// NonlinearFactorGraph graph;
|
||||
// graph.push_back(smartFactor1);
|
||||
// graph.push_back(smartFactor2);
|
||||
// graph.push_back(smartFactor3);
|
||||
// graph.addPrior(x1, level_pose, noisePrior);
|
||||
// graph.addPrior(x2, pose_right, noisePrior);
|
||||
//
|
||||
// Values groundTruth;
|
||||
// groundTruth.insert(x1, level_pose);
|
||||
// groundTruth.insert(x2, pose_right);
|
||||
// groundTruth.insert(x3, pose_above);
|
||||
// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
|
||||
//
|
||||
// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
|
||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
// Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
// Values values;
|
||||
// values.insert(x1, level_pose);
|
||||
// values.insert(x2, pose_right);
|
||||
// // initialize third pose with some noise, we expect it to move back to original pose_above
|
||||
// values.insert(x3, pose_above * noise_pose);
|
||||
// EXPECT( // check that the pose is actually noisy
|
||||
// assert_equal(
|
||||
// Pose3(
|
||||
// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
|
||||
// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
|
||||
// Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
|
||||
//
|
||||
// Values result;
|
||||
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
// result = optimizer.optimize();
|
||||
// EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5));
|
||||
}
|
||||
|
||||
//#ifndef DISABLE_TIMING
|
||||
//#include <gtsam/base/timing.h>
|
||||
//// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor
|
||||
////-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0)
|
||||
////| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0)
|
||||
////| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0)
|
||||
///* *************************************************************************/
|
||||
//TEST( SmartProjectionFactorP, timing ) {
|
||||
//
|
||||
// using namespace vanillaPose;
|
||||
//
|
||||
// // Default cameras for simple derivatives
|
||||
// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
|
||||
//
|
||||
// Rot3 R = Rot3::identity();
|
||||
// Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
|
||||
// Pose3 pose2 = Pose3(R, Point3(1, 0, 0));
|
||||
// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
|
||||
// Pose3 body_P_sensorId = Pose3::identity();
|
||||
//
|
||||
// // one landmarks 1m in front of camera
|
||||
// Point3 landmark1(0, 0, 10);
|
||||
//
|
||||
// Point2Vector measurements_lmk1;
|
||||
//
|
||||
// // Project 2 landmarks into 2 cameras
|
||||
// measurements_lmk1.push_back(cam1.project(landmark1));
|
||||
// measurements_lmk1.push_back(cam2.project(landmark1));
|
||||
//
|
||||
// size_t nrTests = 1000;
|
||||
//
|
||||
// for(size_t i = 0; i<nrTests; i++){
|
||||
// SmartFactorP::shared_ptr smartFactorP(new SmartFactorP(model));
|
||||
// smartFactorP->add(measurements_lmk1[0], x1, sharedKSimple, body_P_sensorId);
|
||||
// smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId);
|
||||
//
|
||||
// Values values;
|
||||
// values.insert(x1, pose1);
|
||||
// values.insert(x2, pose2);
|
||||
// gttic_(SmartFactorP_LINEARIZE);
|
||||
// smartFactorP->linearize(values);
|
||||
// gttoc_(SmartFactorP_LINEARIZE);
|
||||
// }
|
||||
//
|
||||
// for(size_t i = 0; i<nrTests; i++){
|
||||
// SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple));
|
||||
// smartFactor->add(measurements_lmk1[0], x1);
|
||||
// smartFactor->add(measurements_lmk1[1], x2);
|
||||
//
|
||||
// Values values;
|
||||
// values.insert(x1, pose1);
|
||||
// values.insert(x2, pose2);
|
||||
// gttic_(SmartPoseFactor_LINEARIZE);
|
||||
// smartFactor->linearize(values);
|
||||
// gttoc_(SmartPoseFactor_LINEARIZE);
|
||||
// }
|
||||
// tictoc_print_();
|
||||
//}
|
||||
//#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
|
||||
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
|
||||
|
|
|
|||
Loading…
Reference in New Issue