125 lines
4.3 KiB
C++
125 lines
4.3 KiB
C++
// This file is part of SVO - Semi-direct Visual Odometry.
|
|
//
|
|
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
|
|
// (Robotics and Perception Group, University of Zurich, Switzerland).
|
|
//
|
|
// SVO is free software: you can redistribute it and/or modify it under the
|
|
// terms of the GNU General Public License as published by the Free Software
|
|
// Foundation, either version 3 of the License, or any later version.
|
|
//
|
|
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
|
|
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
|
|
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
|
|
//
|
|
// You should have received a copy of the GNU General Public License
|
|
// along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
#include <string.h>
|
|
#include <svo/global.h>
|
|
#include <svo/feature_alignment.h>
|
|
#include <vikit/timer.h>
|
|
#include <vikit/aligned_mem.h>
|
|
#include "test_utils.h"
|
|
#if __ARM_NEON__
|
|
#include <arm_neon.h>
|
|
#endif
|
|
|
|
using namespace Eigen;
|
|
|
|
void generateRefPatchNoWarpInterpolate(const cv::Mat& img, const Vector2d& px,
|
|
cv::Mat& ref_patch_with_border)
|
|
{
|
|
// compute interpolation weights
|
|
const int u_r = floor(px[0]);
|
|
const int v_r = floor(px[1]);
|
|
const float subpix_u = px[0]-u_r;
|
|
const float subpix_v = px[1]-v_r;
|
|
const float wTL = (1.0-subpix_u)*(1.0-subpix_v);
|
|
const float wTR = subpix_u * (1.0-subpix_v);
|
|
const float wBL = (1.0-subpix_u)*subpix_v;
|
|
const float wBR = subpix_u * subpix_v;
|
|
|
|
// loop through search_patch, interpolate
|
|
ref_patch_with_border = cv::Mat(10, 10, CV_8UC1);
|
|
uint8_t* patch_ptr = ref_patch_with_border.data;
|
|
const int stride = img.step.p[0];
|
|
for(int y=0; y<10; ++y)
|
|
{
|
|
uint8_t* img_ptr = (uint8_t*) img.data + (v_r+y-4-1)*stride + u_r-4-1;
|
|
for(int x=0; x<10; ++x, ++patch_ptr, ++img_ptr)
|
|
*patch_ptr = wTL*img_ptr[0] + wTR*img_ptr[1] + wBL*img_ptr[stride] + wBR*img_ptr[stride+1];
|
|
}
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
|
|
std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_0.png");
|
|
printf("Loading image '%s'\n", img_name.c_str());
|
|
cv::Mat img(cv::imread(img_name, 0));
|
|
assert(img.type() == CV_8UC1);
|
|
|
|
// TODO: test on corner/gradient features!
|
|
Vector2d px_true(130.2,120.3);
|
|
|
|
// create reference patch with border
|
|
cv::Mat ref_patch_with_border;
|
|
generateRefPatchNoWarpInterpolate(img, px_true, ref_patch_with_border);
|
|
|
|
// create reference patch, aligned
|
|
uint8_t* ref_patch = vk::aligned_mem::aligned_alloc<uint8_t>(64, 16);
|
|
uint8_t* ref_patch_ptr = ref_patch;
|
|
for(int y=1; y<9; ++y)
|
|
{
|
|
uint8_t* ref_patch_border_ptr = ref_patch_with_border.data + y*10 + 1;
|
|
for(int x=0; x<8; ++x, ++ref_patch_border_ptr, ++ref_patch_ptr)
|
|
*ref_patch_ptr = *ref_patch_border_ptr;
|
|
}
|
|
|
|
Vector2d px_est, px_error(-1.1, -0.8);
|
|
double h_inv;
|
|
vk::Timer t;
|
|
for(int i=0; i<1000; ++i)
|
|
{
|
|
px_est = px_true-px_error;
|
|
Vector2f dir = px_error.normalized().cast<float>();
|
|
svo::feature_alignment::align1D(img, dir, ref_patch_with_border.data, ref_patch, 3, px_est, h_inv);
|
|
}
|
|
Vector2d e = px_est-px_true;
|
|
printf("1000Xalign 1D took %fms, error = %fpx \t (ref i7-W520: 1.982000ms, 0.000033px) \n", t.stop()*1000, e.norm());
|
|
|
|
t.start();
|
|
for(int i=0; i<1000; ++i)
|
|
{
|
|
px_est = px_true-px_error;
|
|
svo::feature_alignment::align2D(img, ref_patch_with_border.data, ref_patch, 3, px_est, true);
|
|
}
|
|
e = px_est-px_true;
|
|
printf("1000Xalign 2D took %fms, error = %fpx \t (ref i7-W520: 2.306000ms, 0.015102px)\n", t.stop()*1000, e.norm());
|
|
|
|
#ifdef __SSE2__
|
|
// Note, this KLT implementation is not invariant to illuminatino changes!
|
|
t.start();
|
|
for(int i=0; i<1000; ++i)
|
|
{
|
|
px_est = px_true-px_error;
|
|
svo::feature_alignment::align2D_SSE2(img, ref_patch_with_border.data, ref_patch, 3, px_est);
|
|
}
|
|
e = px_est-px_true;
|
|
printf("1000Xalign 2D SSE2 %fms, error = %fpx \t (ref i7-W520: 0.460000ms, 0.021881px)\n", t.stop()*1000, e.norm());
|
|
#endif
|
|
|
|
#ifdef __ARM_NEON__
|
|
t.start();
|
|
for(int i=0; i<1000; ++i)
|
|
{
|
|
px_est = px_true-px_error;
|
|
svo::feature_alignment::align2D_NEON(img, ref_patch_with_border.data, ref_patch, 3, px_est);
|
|
}
|
|
e = px_est-px_true;
|
|
printf("1000Xalign 2D NEON %fms, error = %fpx \t (ref Odroid-U2: ?, ?)\n", t.stop()*1000, e.norm());
|
|
#endif
|
|
|
|
return 0;
|
|
}
|