Moved Tensor related Files from CitySLAM
Manohar Paluri
parent
1923778750
commit
c16e6fc0d5
|
|
@ -0,0 +1,37 @@
|
|||
/*
|
||||
* tensorInterface.cpp
|
||||
* @brief Interfacing tensors template library and gtsam
|
||||
* Created on: Feb 12, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include "tensorInterface.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace tensors;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
boost::tuple<int, double, Vector> DLT(const Matrix& A) {
|
||||
|
||||
// Do SVD on A
|
||||
Matrix U, V;
|
||||
Vector S;
|
||||
svd(A, U, S, V);
|
||||
|
||||
// Find minimum column
|
||||
int n = V.size2(), min_j = n - 1, rank = 0;
|
||||
for (int j = 0; j < n; j++)
|
||||
if (S(j) > 1e-9) rank++;
|
||||
double min_S = S(min_j);
|
||||
for (int j = 0; j < n - 1; j++)
|
||||
if (S(j) < min_S) {
|
||||
min_j = j;
|
||||
min_S = S(j);
|
||||
}
|
||||
|
||||
// Return minimum column
|
||||
return boost::tuple<int, double, Vector>(rank, min_S, column_(V, min_j));
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,90 @@
|
|||
/*
|
||||
* tensorInterface.h
|
||||
* @brief Interfacing tensors template library and gtsam
|
||||
* Created on: Feb 12, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include "tensors.h"
|
||||
#include "Matrix.h""
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Reshape 3*3 rank 2 tensor into Matrix */
|
||||
template<class A, class I, class J>
|
||||
Matrix reshape(const tensors::Tensor2Expression<A, I, J>& T, int m, int n) {
|
||||
if (m * n != 9) throw std::invalid_argument(
|
||||
"reshape: incompatible dimensions");
|
||||
Matrix M(m, n);
|
||||
size_t t = 0;
|
||||
for (int j = 0; j < J::dim; j++)
|
||||
for (int i = 0; i < I::dim; i++)
|
||||
M.data()[t++] = T(i, j);
|
||||
return M;
|
||||
}
|
||||
|
||||
/** Reshape Vector into rank 2 tensor */
|
||||
template<int N1, int N2>
|
||||
tensors::Tensor2<N1, N2> reshape2(const Vector& v) {
|
||||
if (v.size() != N1 * N2) throw std::invalid_argument(
|
||||
"reshape2: incompatible dimensions");
|
||||
double data[N1][N2];
|
||||
int t = 0;
|
||||
for (int j = 0; j < N2; j++)
|
||||
for (int i = 0; i < N1; i++)
|
||||
data[j][i] = v(t++);
|
||||
return tensors::Tensor2<N1, N2>(data);
|
||||
}
|
||||
|
||||
/** Reshape rank 3 tensor into Matrix */
|
||||
template<class A, class I, class J, class K>
|
||||
Matrix reshape(const tensors::Tensor3Expression<A, I, J, K>& T, int m, int n) {
|
||||
if (m * n != I::dim * J::dim * K::dim) throw std::invalid_argument(
|
||||
"reshape: incompatible dimensions");
|
||||
Matrix M(m, n);
|
||||
int t = 0;
|
||||
for (int k = 0; k < K::dim; k++)
|
||||
for (int j = 0; j < J::dim; j++)
|
||||
for (int i = 0; i < I::dim; i++)
|
||||
M.data()[t++] = T(i, j, k);
|
||||
return M;
|
||||
}
|
||||
|
||||
/** Reshape Vector into rank 3 tensor */
|
||||
template<int N1, int N2, int N3>
|
||||
tensors::Tensor3<N1, N2, N3> reshape3(const Vector& v) {
|
||||
if (v.size() != N1 * N2 * N3) throw std::invalid_argument(
|
||||
"reshape3: incompatible dimensions");
|
||||
double data[N1][N2][N3];
|
||||
int t = 0;
|
||||
for (int k = 0; k < N3; k++)
|
||||
for (int j = 0; j < N2; j++)
|
||||
for (int i = 0; i < N1; i++)
|
||||
data[k][j][i] = v(t++);
|
||||
return tensors::Tensor3<N1, N2, N3>(data);
|
||||
}
|
||||
|
||||
/** Reshape rank 5 tensor into Matrix */
|
||||
template<class A, class I, class J, class K, class L, class M>
|
||||
Matrix reshape(const tensors::Tensor5Expression<A, I, J, K, L, M>& T, int m,
|
||||
int n) {
|
||||
if (m * n != I::dim * J::dim * K::dim * L::dim * M::dim) throw std::invalid_argument(
|
||||
"reshape: incompatible dimensions");
|
||||
Matrix R(m, n);
|
||||
int t = 0;
|
||||
for (int m = 0; m < M::dim; m++)
|
||||
for (int l = 0; l < L::dim; l++)
|
||||
for (int k = 0; k < K::dim; k++)
|
||||
for (int j = 0; j < J::dim; j++)
|
||||
for (int i = 0; i < I::dim; i++)
|
||||
R.data()[t++] = T(i, j, k, l, m);
|
||||
return R;
|
||||
}
|
||||
|
||||
/**
|
||||
* Find Vector v that minimizes algebraic error A*v
|
||||
* Returns rank of A, minimum error, and corresponding eigenvector
|
||||
*/
|
||||
boost::tuple<int,double,Vector> DLT(const Matrix& A);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
/*
|
||||
* tensors.h
|
||||
* @brief Tensor expression templates based on http://www.gps.caltech.edu/~walter/FTensor/FTensor.pdf
|
||||
* Created on: Feb 10, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace tensors {
|
||||
|
||||
/** index */
|
||||
template<int Dim, char C> struct Index {
|
||||
enum { dim = Dim };
|
||||
};
|
||||
|
||||
} // namespace tensors
|
||||
|
||||
// Expression templates
|
||||
#include "Tensor1Expression.h"
|
||||
#include "Tensor2Expression.h"
|
||||
#include "Tensor3Expression.h"
|
||||
// Tensor4 not needed so far
|
||||
#include "Tensor5Expression.h"
|
||||
|
||||
// Actual tensor classes
|
||||
#include "Tensor1.h"
|
||||
#include "Tensor2.h"
|
||||
#include "Tensor3.h"
|
||||
#include "Tensor4.h"
|
||||
#include "Tensor5.h"
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,104 @@
|
|||
/*
|
||||
* testHomography2.cpp
|
||||
* @brief Test and estimate 2D homographies
|
||||
* Created on: Feb 13, 2010
|
||||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "tensors.h"
|
||||
#include "tensorInterface.h"
|
||||
#include "projectiveGeometry.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace tensors;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Indices
|
||||
|
||||
Index<3, 'a'> a, _a;
|
||||
Index<3, 'b'> b, _b;
|
||||
Index<3, 'c'> c, _c;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Homography2, RealImages)
|
||||
{
|
||||
// 4 point correspondences MATLAB from the floor of bt001.png and bt002.png
|
||||
Correspondence p1(point2h(216.841, 443.220, 1), point2h(213.528, 414.671, 1));
|
||||
Correspondence p2(point2h(252.119, 363.481, 1), point2h(244.614, 348.842, 1));
|
||||
Correspondence p3(point2h(316.614, 414.768, 1), point2h(303.128, 390.000, 1));
|
||||
Correspondence p4(point2h(324.165, 465.463, 1), point2h(308.614, 431.129, 1));
|
||||
|
||||
// Homography obtained using MATLAB code
|
||||
double h[3][3] = { { 0.9075, 0.0031, -0 }, { -0.1165, 0.8288, -0.0004 }, {
|
||||
30.8472, 16.0449, 1 } };
|
||||
Homography2 H(h);
|
||||
|
||||
// CHECK whether they are equivalent
|
||||
CHECK(assert_equivalent(p1.second(b),H(b,a)*p1.first(a),0.05))
|
||||
CHECK(assert_equivalent(p2.second(b),H(b,a)*p2.first(a),0.05))
|
||||
CHECK(assert_equivalent(p3.second(b),H(b,a)*p3.first(a),0.05))
|
||||
CHECK(assert_equivalent(p4.second(b),H(b,a)*p4.first(a),0.05))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Homography test case
|
||||
// 4 trivial correspondences of a translating square
|
||||
Correspondence p1(point2h(0, 0, 1), point2h(4, 5, 1));
|
||||
Correspondence p2(point2h(1, 0, 1), point2h(5, 5, 1));
|
||||
Correspondence p3(point2h(1, 1, 1), point2h(5, 6, 1));
|
||||
Correspondence p4(point2h(0, 1, 1), point2h(4, 6, 1));
|
||||
|
||||
double h[3][3] = { { 1, 0, 4 }, { 0, 1, 5 }, { 0, 0, 1 } };
|
||||
Homography2 H(h);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Homography2, TestCase)
|
||||
{
|
||||
// Check homography
|
||||
list<Correspondence> correspondences;
|
||||
correspondences += p1, p2, p3, p4;
|
||||
BOOST_FOREACH(const Correspondence& p, correspondences)
|
||||
CHECK(assert_equality(p.second(b),H(_a,b) * p.first(a)))
|
||||
|
||||
// Check a line
|
||||
Line2h l1 = line2h(1, 0, -1); // in a
|
||||
Line2h l2 = line2h(1, 0, -5); // x==5 in b
|
||||
CHECK(assert_equality(l1(a), H(a,b)*l2(b)))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Homography2, Estimate)
|
||||
{
|
||||
list<Correspondence> correspondences;
|
||||
correspondences += p1, p2, p3, p4;
|
||||
Homography2 estimatedH = estimateHomography2(correspondences);
|
||||
CHECK(assert_equivalent(H(_a,b),estimatedH(_a,b)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Homography2, EstimateReverse)
|
||||
{
|
||||
double h[3][3] = { { 1, 0, -4 }, { 0, 1, -5 }, { 0, 0, 1 } };
|
||||
Homography2 reverse(h);
|
||||
|
||||
list<Correspondence> correspondences;
|
||||
correspondences += p1.swap(), p2.swap(), p3.swap(), p4.swap();
|
||||
Homography2 estimatedH = estimateHomography2(correspondences);
|
||||
CHECK(assert_equality(reverse(_a,b),estimatedH(_a,b)*(1.0/estimatedH(2,2))));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue