add banchmark image alignment 2

include/vision.h

@@ -0,0 +1,80 @@
+/*
+ * vision.h
+ *
+ *  Created on: May 14, 2013
+ *      Author: cforster
+ */
+
+#ifndef VIKIT_VISION_H_
+#define VIKIT_VISION_H_
+
+#include <aligned_mem.h>
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+
+namespace vk
+{
+
+//! Return value between 0 and 1
+//! WARNING This function does not check whether the x/y is within the border
+inline float
+interpolateMat_32f(const cv::Mat& mat, float u, float v)
+{
+  assert(mat.type()==CV_32F);
+  float x = floor(u);
+  float y = floor(v);
+  float subpix_x = u-x;
+  float subpix_y = v-y;
+  float wx0 = 1.0-subpix_x;
+  float wx1 =  subpix_x;
+  float wy0 = 1.0-subpix_y;
+  float wy1 =  subpix_y;
+
+  float val00 = mat.at<float>(y,x);
+  float val10 = mat.at<float>(y,x+1);
+  float val01 = mat.at<float>(y+1,x);
+  float val11 = mat.at<float>(y+1,x+1);
+  return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11;
+}
+
+//! Return value between 0 and 255
+//! WARNING This function does not check whether the x/y is within the border
+inline float
+interpolateMat_8u(const cv::Mat& mat, float u, float v)
+{
+  if(mat.empty())return 0.0;
+  if(mat.type()!=CV_8U)return 0.0;
+  int x = floor(u);
+  int y = floor(v);
+  float subpix_x = u-x;
+  float subpix_y = v-y;
+
+  float w00 = (1.0f-subpix_x)*(1.0f-subpix_y);
+  float w01 = (1.0f-subpix_x)*subpix_y;
+  float w10 = subpix_x*(1.0f-subpix_y);
+  float w11 = 1.0f - w00 - w01 - w10;
+
+  const int stride = mat.step.p[0];
+  unsigned char* ptr = mat.data + y*stride + x;
+  if(isnan(w00) || isnan(w01 )||isnan( w10) || isnan(w11) || isinf(w00) || isinf(w01)|| isinf(w10)|| isinf(w11))return 0.0;
+  if(ptr[0] == NULL || ptr[1]== NULL || ptr[stride]==NULL ||ptr[stride+1]==NULL)return 0.0;
+  return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1];
+}
+
+void halfSample(const cv::Mat& in, cv::Mat& out);
+
+float shiTomasiScore(const cv::Mat& img, int u, int v);
+
+void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst);
+
+#ifdef __SSE2__
+
+/// Used to convert a Kinect depthmap
+/// Code by Christian Kerl DVO, GPL Licence
+void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale);
+
+#endif
+
+} // namespace vk
+
+#endif // VIKIT_VISION_H_