added InverseDepth factor based on Montiel/Civera's paramaterization. MOntiel06rss, Civera08tro
Chris Beall
parent
55adfb2082
commit
93d9023a61
|
|
@ -0,0 +1,179 @@
|
|||
|
||||
/**
|
||||
* @file InvDepthCamera3.h
|
||||
* @brief
|
||||
* @author Chris Beall
|
||||
* @date Apr 14, 2012
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <boost/optional.hpp>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/base/LieScalar.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A pinhole camera class that has a Pose3 and a Calibration.
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template <class CALIBRATION>
|
||||
class InvDepthCamera3 {
|
||||
private:
|
||||
Pose3 pose_;
|
||||
boost::shared_ptr<CALIBRATION> k_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor */
|
||||
InvDepthCamera3() {}
|
||||
|
||||
/** constructor with pose and calibration */
|
||||
InvDepthCamera3(const Pose3& pose, const boost::shared_ptr<CALIBRATION>& k) :
|
||||
pose_(pose),k_(k) {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
virtual ~InvDepthCamera3() {}
|
||||
|
||||
/// return pose
|
||||
inline Pose3& pose() { return pose_; }
|
||||
|
||||
/// return calibration
|
||||
inline const boost::shared_ptr<CALIBRATION>& calibration() const { return k_; }
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "") const {
|
||||
pose_.print("pose3");
|
||||
k_.print("calibration");
|
||||
}
|
||||
|
||||
static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) {
|
||||
gtsam::Point3 ray_base(pw.vector().segment(0,3));
|
||||
double theta = pw(3), phi = pw(4);
|
||||
double rho = inv.value(); // inverse depth
|
||||
gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
|
||||
return ray_base + m/rho;
|
||||
}
|
||||
|
||||
/** project a point from world InvDepth parameterization to the image
|
||||
* @param pw is a point in the world coordinate
|
||||
* @param H1 is the jacobian w.r.t. [pose3 calibration]
|
||||
* @param H2 is the jacobian w.r.t. inv_depth_landmark
|
||||
*/
|
||||
inline gtsam::Point2 project(const gtsam::LieVector& pw,
|
||||
const gtsam::LieScalar& inv_depth,
|
||||
boost::optional<gtsam::Matrix&> H1 = boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2 = boost::none,
|
||||
boost::optional<gtsam::Matrix&> H3 = boost::none) const {
|
||||
|
||||
gtsam::Point3 ray_base(pw.vector().segment(0,3));
|
||||
double theta = pw(3), phi = pw(4);
|
||||
double rho = inv_depth.value(); // inverse depth
|
||||
gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
|
||||
const gtsam::Point3 landmark = ray_base + m/rho;
|
||||
|
||||
gtsam::PinholeCamera<CALIBRATION> camera(pose_, *k_);
|
||||
|
||||
if (!H1 && !H2 && !H3) {
|
||||
gtsam::Point2 uv= camera.project(landmark);
|
||||
return uv;
|
||||
}
|
||||
else {
|
||||
gtsam::Matrix J2;
|
||||
gtsam::Point2 uv= camera.project(landmark,H1, J2);
|
||||
if (H1) {
|
||||
*H1 = (*H1) * gtsam::eye(6);
|
||||
}
|
||||
|
||||
double cos_theta = cos(theta);
|
||||
double sin_theta = sin(theta);
|
||||
double cos_phi = cos(phi);
|
||||
double sin_phi = sin(phi);
|
||||
double rho2 = rho * rho;
|
||||
|
||||
if (H2) {
|
||||
double H11 = 1;
|
||||
double H12 = 0;
|
||||
double H13 = 0;
|
||||
double H14 = -cos_phi*sin_theta/rho;
|
||||
double H15 = -cos_theta*sin_phi/rho;
|
||||
|
||||
double H21 = 0;
|
||||
double H22 = 1;
|
||||
double H23 = 0;
|
||||
double H24 = cos_phi*cos_theta/rho;
|
||||
double H25 = -sin_phi*sin_theta/rho;
|
||||
|
||||
double H31 = 0;
|
||||
double H32 = 0;
|
||||
double H33 = 1;
|
||||
double H34 = 0;
|
||||
double H35 = cos_phi/rho;
|
||||
|
||||
*H2 = J2 * gtsam::Matrix_(3,5,
|
||||
H11, H12, H13, H14, H15,
|
||||
H21, H22, H23, H24, H25,
|
||||
H31, H32, H33, H34, H35);
|
||||
}
|
||||
if(H3) {
|
||||
double H16 = -cos_phi*cos_theta/rho2;
|
||||
double H26 = -cos_phi*sin_theta/rho2;
|
||||
double H36 = -sin_phi/rho2;
|
||||
*H3 = J2 * gtsam::Matrix_(3,1,
|
||||
H16,
|
||||
H26,
|
||||
H36);
|
||||
}
|
||||
return uv;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* backproject a 2-dimensional point to an Inverse Depth landmark
|
||||
* useful for point initialization
|
||||
*/
|
||||
|
||||
inline std::pair<gtsam::LieVector, gtsam::LieScalar> backproject(const gtsam::Point2& pi, const double depth) const {
|
||||
const gtsam::Point2 pn = k_->calibrate(pi);
|
||||
gtsam::Point3 pc(pn.x(), pn.y(), 1.0);
|
||||
pc = pc/pc.norm();
|
||||
gtsam::Point3 pw = pose_.transform_from(pc);
|
||||
const gtsam::Point3& pt = pose_.translation();
|
||||
gtsam::Point3 ray = pw - pt;
|
||||
double theta = atan2(ray.y(), ray.x()); // longitude
|
||||
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
|
||||
return std::make_pair(gtsam::LieVector(5, pt.x(),pt.y(),pt.z(), theta, phi),
|
||||
gtsam::LieScalar(1./depth));
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(pose_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k_);
|
||||
}
|
||||
/// @}
|
||||
};
|
||||
}
|
||||
|
|
@ -0,0 +1,111 @@
|
|||
|
||||
/**
|
||||
* @file InvDepthFactor3.h
|
||||
* @brief Inverse Depth Factor
|
||||
* @author Chris Beall
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam_unstable/slam/InvDepthCamera3.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template<class POSE, class LANDMARK, class INVDEPTH>
|
||||
class InvDepthFactor3: public gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
|
||||
protected:
|
||||
|
||||
// Keep a copy of measurement and calibration for I/O
|
||||
gtsam::Point2 measured_; ///< 2D measurement
|
||||
boost::shared_ptr<gtsam::Cal3_S2> K_; ///< shared pointer to calibration object
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for base class type
|
||||
typedef gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
|
||||
|
||||
/// shorthand for this class
|
||||
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// Default constructor
|
||||
InvDepthFactor3() : K_(new gtsam::Cal3_S2(444, 555, 666, 777, 888)) {}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* TODO: Mark argument order standard (keys, measurement, parameters)
|
||||
* @param z is the 2 dimensional location of point in image (the measurement)
|
||||
* @param model is the standard deviation
|
||||
* @param j_pose is basically the frame number
|
||||
* @param j_landmark is the index of the landmark
|
||||
* @param K shared pointer to the constant calibration
|
||||
*/
|
||||
InvDepthFactor3(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model,
|
||||
const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const gtsam::shared_ptrK& K) :
|
||||
Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~InvDepthFactor3() {}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
*/
|
||||
void print(const std::string& s = "InvDepthFactor3",
|
||||
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
|
||||
Base::print(s, keyFormatter);
|
||||
measured_.print(s + ".z");
|
||||
}
|
||||
|
||||
/// equals
|
||||
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
|
||||
const This *e = dynamic_cast<const This*>(&p);
|
||||
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol);
|
||||
}
|
||||
|
||||
/// Evaluate error h(x)-z and optionally derivatives
|
||||
gtsam::Vector evaluateError(const POSE& pose, const gtsam::LieVector& point, const INVDEPTH& invDepth,
|
||||
boost::optional<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H3=boost::none) const {
|
||||
try {
|
||||
InvDepthCamera3<gtsam::Cal3_S2> camera(pose, K_);
|
||||
gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_);
|
||||
return reprojectionError.vector();
|
||||
} catch( CheiralityException& e) {
|
||||
if (H1) *H1 = gtsam::zeros(2,6);
|
||||
if (H2) *H2 = gtsam::zeros(2,5);
|
||||
if (H3) *H2 = gtsam::zeros(2,1);
|
||||
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
|
||||
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
|
||||
return gtsam::ones(2) * 2.0 * K_->fx();
|
||||
}
|
||||
}
|
||||
|
||||
/** return the measurement */
|
||||
const gtsam::Point2& imagePoint() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
/** return the calibration object */
|
||||
inline const gtsam::Cal3_S2::shared_ptr calibration() const {
|
||||
return K_;
|
||||
}
|
||||
|
||||
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(measured_);
|
||||
ar & BOOST_SERIALIZATION_NVP(K_);
|
||||
}
|
||||
};
|
||||
} // \ namespace gtsam
|
||||
|
|
@ -0,0 +1,223 @@
|
|||
/*
|
||||
* testInvDepthFactor.cpp
|
||||
*
|
||||
* Created on: Apr 13, 2012
|
||||
* Author: cbeall3
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/slam/InvDepthFactor3.h>
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
|
||||
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
|
||||
SimpleCamera level_camera(level_pose, *K);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactor, Project1) {
|
||||
|
||||
// landmark 5 meters infront of camera
|
||||
Point3 landmark(5, 0, 1);
|
||||
|
||||
Point2 expected_uv = level_camera.project(landmark);
|
||||
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
|
||||
LieVector inv_landmark(5, 1., 0., 1., 0., 0.);
|
||||
LieScalar inv_depth(1./4);
|
||||
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
|
||||
CHECK(assert_equal(expected_uv, actual_uv));
|
||||
CHECK(assert_equal(Point2(640,480), actual_uv));
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactor, Project2) {
|
||||
|
||||
// landmark 1m to the left and 1m up from camera
|
||||
// inv landmark xyz is same as camera xyz, so depth actually doesn't matter
|
||||
Point3 landmark(1, 1, 2);
|
||||
Point2 expected = level_camera.project(landmark);
|
||||
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
|
||||
LieVector diag_landmark(5, 0., 0., 1., M_PI/4., atan(1/sqrt(2)));
|
||||
LieScalar inv_depth(1/sqrt(3));
|
||||
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactor, Project3) {
|
||||
|
||||
// landmark 1m to the left and 1m up from camera
|
||||
// inv depth landmark xyz at origion
|
||||
Point3 landmark(1, 1, 2);
|
||||
Point2 expected = level_camera.project(landmark);
|
||||
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
|
||||
LieVector diag_landmark(5, 0., 0., 0., M_PI/4., atan(2./sqrt(2)));
|
||||
LieScalar inv_depth( 1./sqrt(1+1+4));
|
||||
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactor, Project4) {
|
||||
|
||||
// landmark 4m to the left and 1m up from camera
|
||||
// inv depth landmark xyz at origion
|
||||
Point3 landmark(1, 4, 2);
|
||||
Point2 expected = level_camera.project(landmark);
|
||||
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
|
||||
LieVector diag_landmark(5, 0., 0., 0., atan(4/1), atan(2./sqrt(1+16)));
|
||||
LieScalar inv_depth(1./sqrt(1+16+4));
|
||||
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& inv_depth) {
|
||||
return InvDepthCamera3<Cal3_S2>(pose,K).project(landmark, inv_depth); }
|
||||
|
||||
TEST( InvDepthFactor, Dproject_pose)
|
||||
{
|
||||
LieVector landmark(6,0.1,0.2,0.3, 0.1,0.2);
|
||||
LieScalar inv_depth(1./4);
|
||||
Matrix expected = numericalDerivative31<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
|
||||
Matrix actual;
|
||||
Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none);
|
||||
CHECK(assert_equal(expected,actual,1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactor, Dproject_landmark)
|
||||
{
|
||||
LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2);
|
||||
LieScalar inv_depth(1./4);
|
||||
Matrix expected = numericalDerivative32<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
|
||||
Matrix actual;
|
||||
Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none);
|
||||
CHECK(assert_equal(expected,actual,1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactor, Dproject_inv_depth)
|
||||
{
|
||||
LieVector landmark(5,0.1,0.2,0.3, 0.1,0.2);
|
||||
LieScalar inv_depth(1./4);
|
||||
Matrix expected = numericalDerivative33<Point2,Pose3,LieVector>(project_,level_pose, landmark, inv_depth);
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
|
||||
Matrix actual;
|
||||
Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual);
|
||||
CHECK(assert_equal(expected,actual,1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(InvDepthFactor, backproject)
|
||||
{
|
||||
LieVector expected(5,0.,0.,1., 0.1,0.2);
|
||||
LieScalar inv_depth(1./4);
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
|
||||
Point2 z = inv_camera.project(expected, inv_depth);
|
||||
|
||||
LieVector actual_vec;
|
||||
LieScalar actual_inv;
|
||||
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4);
|
||||
CHECK(assert_equal(expected,actual_vec,1e-7));
|
||||
CHECK(assert_equal(inv_depth,actual_inv,1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(InvDepthFactor, backproject2)
|
||||
{
|
||||
// backwards facing camera
|
||||
LieVector expected(5,-5.,-5.,2., 3., -0.1);
|
||||
LieScalar inv_depth(1./10);
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
|
||||
Point2 z = inv_camera.project(expected, inv_depth);
|
||||
|
||||
LieVector actual_vec;
|
||||
LieScalar actual_inv;
|
||||
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10);
|
||||
CHECK(assert_equal(expected,actual_vec,1e-7));
|
||||
CHECK(assert_equal(inv_depth,actual_inv,1e-7));
|
||||
}
|
||||
|
||||
static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
|
||||
typedef InvDepthFactor3<Pose3, LieVector, LieScalar> InverseDepthFactor;
|
||||
typedef NonlinearEquality<Pose3> PoseConstraint;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactor, optimize) {
|
||||
|
||||
// landmark 5 meters infront of camera
|
||||
Point3 landmark(5, 0, 1);
|
||||
|
||||
Point2 expected_uv = level_camera.project(landmark);
|
||||
|
||||
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
|
||||
LieVector inv_landmark(5, 1., 0., 1., 0., 0.);
|
||||
LieScalar inv_depth(1./4);
|
||||
|
||||
gtsam::NonlinearFactorGraph graph;
|
||||
Values initial;
|
||||
|
||||
InverseDepthFactor::shared_ptr factor(new InverseDepthFactor(expected_uv, sigma,
|
||||
Symbol('x',1), Symbol('l',1), Symbol('d',1), K));
|
||||
graph.push_back(factor);
|
||||
graph.add(PoseConstraint(Symbol('x',1),level_pose));
|
||||
initial.insert(Symbol('x',1), level_pose);
|
||||
initial.insert(Symbol('l',1), inv_landmark);
|
||||
initial.insert(Symbol('d',1), inv_depth);
|
||||
|
||||
LevenbergMarquardtParams lmParams;
|
||||
Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
|
||||
CHECK(assert_equal(initial, result, 1e-9));
|
||||
|
||||
/// Add a second camera
|
||||
|
||||
// add a camera 1 meter to the right
|
||||
Pose3 right_pose = level_pose * Pose3(Rot3(), Point3(2,0,0));
|
||||
SimpleCamera right_camera(right_pose, *K);
|
||||
|
||||
InvDepthCamera3<Cal3_S2> right_inv_camera(right_pose, K);
|
||||
|
||||
Point3 landmark1(6,0,1);
|
||||
Point2 right_uv = right_camera.project(landmark1);
|
||||
|
||||
|
||||
InverseDepthFactor::shared_ptr factor1(new InverseDepthFactor(right_uv, sigma,
|
||||
Symbol('x',2), Symbol('l',1),Symbol('d',1),K));
|
||||
graph.push_back(factor1);
|
||||
|
||||
graph.add(PoseConstraint(Symbol('x',2),right_pose));
|
||||
|
||||
initial.insert(Symbol('x',2), right_pose);
|
||||
|
||||
// TODO: need to add priors to make this work with
|
||||
// Values result2 = optimize<NonlinearFactorGraph>(graph, initial,
|
||||
// NonlinearOptimizationParameters(),MULTIFRONTAL, GN);
|
||||
|
||||
Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
|
||||
Point3 l1_result2 = InvDepthCamera3<Cal3_S2>::invDepthTo3D(
|
||||
result2.at<LieVector>(Symbol('l',1)),
|
||||
result2.at<LieScalar>(Symbol('d',1)));
|
||||
|
||||
CHECK(assert_equal(landmark1, l1_result2, 1e-9));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue