Merge pull request #962 from borglab/feature/wrap_EssentialMatrixConstraint
wrapped and tested EssentialMatrixConstraintrelease/4.3a0
commit
4abe1a135c
|
@ -1,7 +1,20 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010-2014, 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 EssentialMatrixFactor.cpp
|
||||
* @file EssentialMatrixFactor.h
|
||||
* @brief EssentialMatrixFactor class
|
||||
* @author Frank Dellaert
|
||||
* @author Ayush Baid
|
||||
* @author Akshay Krishnan
|
||||
* @date December 17, 2013
|
||||
*/
|
||||
|
||||
|
|
|
@ -196,6 +196,21 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
|
|||
EssentialMatrixFactor(size_t key, const gtsam::Point2& pA,
|
||||
const gtsam::Point2& pB,
|
||||
const gtsam::noiseModel::Base* noiseModel);
|
||||
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const;
|
||||
Vector evaluateError(const gtsam::EssentialMatrix& E) const;
|
||||
};
|
||||
|
||||
#include <gtsam/slam/EssentialMatrixConstraint.h>
|
||||
virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor {
|
||||
EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE,
|
||||
const gtsam::noiseModel::Base *model);
|
||||
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
|
||||
gtsam::DefaultKeyFormatter) const;
|
||||
bool equals(const gtsam::EssentialMatrixConstraint& other, double tol) const;
|
||||
Vector evaluateError(const gtsam::Pose3& p1, const gtsam::Pose3& p2) const;
|
||||
const gtsam::EssentialMatrix& measured() const;
|
||||
};
|
||||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
|
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testEssentialMatrixConstraint.cpp
|
||||
* @file TestEssentialMatrixConstraint.cpp
|
||||
* @brief Unit tests for EssentialMatrixConstraint Class
|
||||
* @author Frank Dellaert
|
||||
* @author Pablo Alcantarilla
|
||||
|
|
|
@ -0,0 +1,47 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
visual_isam unit tests.
|
||||
Author: Frank Dellaert & Pablo Alcantarilla
|
||||
"""
|
||||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam import (EssentialMatrix, EssentialMatrixConstraint, Point3, Pose3,
|
||||
Rot3, Unit3, symbol)
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
|
||||
class TestVisualISAMExample(GtsamTestCase):
|
||||
def test_VisualISAMExample(self):
|
||||
|
||||
# Create a factor
|
||||
poseKey1 = symbol('x', 1)
|
||||
poseKey2 = symbol('x', 2)
|
||||
trueRotation = Rot3.RzRyRx(0.15, 0.15, -0.20)
|
||||
trueTranslation = Point3(+0.5, -1.0, +1.0)
|
||||
trueDirection = Unit3(trueTranslation)
|
||||
E = EssentialMatrix(trueRotation, trueDirection)
|
||||
model = gtsam.noiseModel.Isotropic.Sigma(5, 0.25)
|
||||
factor = EssentialMatrixConstraint(poseKey1, poseKey2, E, model)
|
||||
|
||||
# Create a linearization point at the zero-error point
|
||||
pose1 = Pose3(Rot3.RzRyRx(0.00, -0.15, 0.30), Point3(-4.0, 7.0, -10.0))
|
||||
pose2 = Pose3(
|
||||
Rot3.RzRyRx(0.179693265735950, 0.002945368776519,
|
||||
0.102274823253840),
|
||||
Point3(-3.37493895, 6.14660244, -8.93650986))
|
||||
|
||||
expected = np.zeros((5, 1))
|
||||
actual = factor.evaluateError(pose1, pose2)
|
||||
self.gtsamAssertEquals(actual, expected, 1e-8)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
Loading…
Reference in New Issue