diff --git a/binding.gyp b/binding.gyp index 8cd66bf1..deb48a76 100755 --- a/binding.gyp +++ b/binding.gyp @@ -20,7 +20,14 @@ "src/Calib3D.cc", "src/ImgProc.cc", "src/Stereo.cc", - "src/LDAWrap.cc" + "src/LDAWrap.cc", + "src/CMT/CMT.cpp", + "src/CMT/Consensus.cpp", + "src/CMT/fastcluster/fastcluster.cpp", + "src/CMT/Fusion.cpp", + "src/CMT/Matcher.cpp", + "src/CMT/Tracker.cpp", + "src/CMT/common.cpp" ], "libraries": [ diff --git a/binding.gyp~ b/binding.gyp~ new file mode 100755 index 00000000..8cd66bf1 --- /dev/null +++ b/binding.gyp~ @@ -0,0 +1,84 @@ +{ + "targets": [{ + + "target_name": "opencv", + + "sources": [ + "src/init.cc", + "src/Matrix.cc", + "src/OpenCV.cc", + "src/CascadeClassifierWrap.cc", + "src/Contours.cc", + "src/Point.cc", + "src/VideoCaptureWrap.cc", + "src/CamShift.cc", + "src/HighGUI.cc", + "src/FaceRecognizer.cc", + "src/Features2d.cc", + "src/BackgroundSubtractor.cc", + "src/Constants.cc", + "src/Calib3D.cc", + "src/ImgProc.cc", + "src/Stereo.cc", + "src/LDAWrap.cc" + ], + + "libraries": [ + "= 2.3.1\" )", + "-Wall" + ] + }], + [ "OS==\"win\"", { + "cflags": [ + "= 2.4.9\" )", + "-Wall" + ], + "msvs_settings": { + "VCCLCompilerTool": { + "ExceptionHandling": "2", + "DisableSpecificWarnings": [ "4530", "4506", "4244" ], + }, + } + }], + [ # cflags on OS X are stupid and have to be defined like this + "OS==\"mac\"", { + "xcode_settings": { + "OTHER_CFLAGS": [ + "-mmacosx-version-min=10.7", + "-std=c++11", + "-stdlib=libc++", + ">', x, ':', '[top-left-x: ', ob[0], ', top-left-y: ', ob[1], ', width: ', ob[2], ', height: ', ob[3], ', center-x: ', Math.round(ob[4]), ', center-y: ', Math.round(ob[5]), ']'); + m2.rectangle([ob[0], ob[1]], [ob[2], ob[3]]); + m2.save('./out-motiontrack-' + x + '.jpg'); + if (x<241) + iter(); + }); + } + iter(); +}); diff --git a/src/CMT/CMT.cpp b/src/CMT/CMT.cpp new file mode 100644 index 00000000..4eb61c02 --- /dev/null +++ b/src/CMT/CMT.cpp @@ -0,0 +1,266 @@ +#include "CMT.h" +#include "../OpenCV.h" +#include "../Matrix.h" + +#include +#include + +Nan::Persistent CMT::constructor; + +void CMT::Init(Local target) { + Nan::HandleScope scope; + + // Constructor + Local ctor = Nan::New(CMT::New); + constructor.Reset(ctor); + ctor->InstanceTemplate()->SetInternalFieldCount(1); + ctor->SetClassName(Nan::New("CMT").ToLocalChecked()); + + // Prototype + // Local proto = constructor->PrototypeTemplate(); + + Nan::SetPrototypeMethod(ctor, "ctrack", cmtTrack); + + target->Set(Nan::New("CMT").ToLocalChecked(), ctor->GetFunction()); +} + +NAN_METHOD(CMT::New) { + Nan::HandleScope scope; + + if (info.This()->InternalFieldCount() == 0){ + JSTHROW_TYPE("Cannot Instantiate without new") + } + + Matrix* m = Nan::ObjectWrap::Unwrap(info[0]->ToObject()); + Rect r; + + if (info[1]->IsArray()){ + Local v8rec = info[1]->ToObject(); + r = Rect( + v8rec->Get(0)->IntegerValue(), + v8rec->Get(1)->IntegerValue(), + v8rec->Get(2)->IntegerValue() - v8rec->Get(0)->IntegerValue(), + v8rec->Get(3)->IntegerValue() - v8rec->Get(1)->IntegerValue()); + } else { + JSTHROW_TYPE("Must pass rectangle to track") + } + + CMT *to = new CMT(m->mat, r); + + to->Wrap(info.This()); + info.GetReturnValue().Set(info.This()); +} + +CMT::CMT(Mat image, Rect rect){ + Mat im0_gray; + + if (image.channels() > 1) { + cvtColor(image, im0_gray, CV_BGR2GRAY); + } else { + im0_gray = image; + } + + initialize(im0_gray, rect); +} + +NAN_METHOD(CMT::cmtTrack){ + SETUP_FUNCTION(CMT) + + if (info.Length() != 1){ + Nan::ThrowTypeError("track takes an image param"); + return; + } + + Matrix *im = Nan::ObjectWrap::Unwrap(info[0]->ToObject()); + Mat im0_gray; + Mat image = im->mat; + + if (image.channels() > 1) { + cvtColor(image, im0_gray, CV_BGR2GRAY); + } else { + im0_gray = image; + } + self->processFrame(im0_gray); + + Rect r = self->bb_rot.boundingRect(); + + v8::Local arr = Nan::New(4); + + arr->Set(0, Nan::New(r.x)); + arr->Set(1, Nan::New(r.y)); + arr->Set(2, Nan::New(r.width)); + arr->Set(3, Nan::New(r.height)); + arr->Set(4, Nan::New(self->oCenter.x)); + arr->Set(5, Nan::New(self->oCenter.y)); + + info.GetReturnValue().Set(arr); +} + +void CMT::initialize(const Mat im_gray, const Rect rect) +{ + str_detector = "FAST"; + str_descriptor = "BRISK"; + + //Remember initial size + size_initial = rect.size(); + + //Remember initial image + im_prev = im_gray; + + //Compute center of rect + Point2f center = Point2f(rect.x + rect.width/2.0, rect.y + rect.height/2.0); + + //Initialize rotated bounding box + bb_rot = RotatedRect(center, size_initial, 0.0); + + //Initialize detector and descriptor +#if CV_MAJOR_VERSION > 2 + detector = cv::FastFeatureDetector::create(); + descriptor = cv::BRISK::create(); +#else + detector = FeatureDetector::create(str_detector); + descriptor = DescriptorExtractor::create(str_descriptor); +#endif + + //Get initial keypoints in whole image and compute their descriptors + vector keypoints; + detector->detect(im_gray, keypoints); + + //Divide keypoints into foreground and background keypoints according to selection + vector keypoints_fg; + vector keypoints_bg; + + for (size_t i = 0; i < keypoints.size(); i++) + { + KeyPoint k = keypoints[i]; + Point2f pt = k.pt; + + if (pt.x > rect.x && pt.y > rect.y && pt.x < rect.br().x && pt.y < rect.br().y) + { + keypoints_fg.push_back(k); + } + + else + { + keypoints_bg.push_back(k); + } + + } + + //Create foreground classes + vector classes_fg; + classes_fg.reserve(keypoints_fg.size()); + for (size_t i = 0; i < keypoints_fg.size(); i++) + { + classes_fg.push_back(i); + } + + //Compute foreground/background features + Mat descs_fg; + Mat descs_bg; + descriptor->compute(im_gray, keypoints_fg, descs_fg); + descriptor->compute(im_gray, keypoints_bg, descs_bg); + + //Only now is the right time to convert keypoints to points, as compute() might remove some keypoints + vector points_fg; + vector points_bg; + + for (size_t i = 0; i < keypoints_fg.size(); i++) + { + points_fg.push_back(keypoints_fg[i].pt); + } + + for (size_t i = 0; i < keypoints_bg.size(); i++) + { + points_bg.push_back(keypoints_bg[i].pt); + } + + //Create normalized points + vector points_normalized; + for (size_t i = 0; i < points_fg.size(); i++) + { + points_normalized.push_back(points_fg[i] - center); + } + + //Initialize matcher + matcher.initialize(points_normalized, descs_fg, classes_fg, descs_bg, center); + + //Initialize consensus + consensus.initialize(points_normalized); + + //Create initial set of active keypoints + for (size_t i = 0; i < keypoints_fg.size(); i++) + { + points_active.push_back(keypoints_fg[i].pt); + classes_active = classes_fg; + } +} + +void CMT::processFrame(Mat im_gray) { + //Track keypoints + vector points_tracked; + vector status; + tracker.track(im_prev, im_gray, points_active, points_tracked, status); + + //keep only successful classes + vector classes_tracked; + for (size_t i = 0; i < classes_active.size(); i++) + { + if (status[i]) + { + classes_tracked.push_back(classes_active[i]); + } + + } + + //Detect keypoints, compute descriptors + vector keypoints; + detector->detect(im_gray, keypoints); + + Mat descriptors; + descriptor->compute(im_gray, keypoints, descriptors); + + //Match keypoints globally + vector points_matched_global; + vector classes_matched_global; + matcher.matchGlobal(keypoints, descriptors, points_matched_global, classes_matched_global); + + //Fuse tracked and globally matched points + vector points_fused; + vector classes_fused; + fusion.preferFirst(points_tracked, classes_tracked, points_matched_global, classes_matched_global, + points_fused, classes_fused); + + //Estimate scale and rotation from the fused points + float scale; + float rotation; + consensus.estimateScaleRotation(points_fused, classes_fused, scale, rotation); + + //Find inliers and the center of their votes + Point2f center; + vector points_inlier; + vector classes_inlier; + consensus.findConsensus(points_fused, classes_fused, scale, rotation, + center, points_inlier, classes_inlier); + oCenter = center; + + //Match keypoints locally + vector points_matched_local; + vector classes_matched_local; + matcher.matchLocal(keypoints, descriptors, center, scale, rotation, points_matched_local, classes_matched_local); + + //Clear active points + points_active.clear(); + classes_active.clear(); + + //Fuse locally matched points and inliers + fusion.preferFirst(points_matched_local, classes_matched_local, points_inlier, classes_inlier, points_active, classes_active); +// points_active = points_fused; +// classes_active = classes_fused; + + //TODO: Use theta to suppress result + bb_rot = RotatedRect(center, size_initial * scale, rotation/CV_PI * 180); + + //Remember current image + im_prev = im_gray; +} diff --git a/src/CMT/CMT.cpp~ b/src/CMT/CMT.cpp~ new file mode 100644 index 00000000..957f4555 --- /dev/null +++ b/src/CMT/CMT.cpp~ @@ -0,0 +1,266 @@ +#include "CMT.h" +#include "../OpenCV.h" +#include "../Matrix.h" + +#include +#include + +Nan::Persistent CMT::constructor; + +void CMT::Init(Local target) { + Nan::HandleScopeasd scope; + + // Constructor + Local ctor = Nan::New(CMT::New); + constructor.Reset(ctor); + ctor->InstanceTemplate()->SetInternalFieldCount(1); + ctor->SetClassName(Nan::New("CMT").ToLocalChecked()); + + // Prototype + // Local proto = constructor->PrototypeTemplate(); + + Nan::SetPrototypeMethod(ctor, "ctrack", cmtTrack); + + target->Set(Nan::New("CMT").ToLocalChecked(), ctor->GetFunction()); +} + +NAN_METHOD(CMT::New) { + Nan::HandleScope scope; + + if (info.This()->InternalFieldCount() == 0){ + JSTHROW_TYPE("Cannot Instantiate without new") + } + + Matrix* m = Nan::ObjectWrap::Unwrap(info[0]->ToObject()); + Rect r; + + if (info[1]->IsArray()){ + Local v8rec = info[1]->ToObject(); + r = Rect( + v8rec->Get(0)->IntegerValue(), + v8rec->Get(1)->IntegerValue(), + v8rec->Get(2)->IntegerValue() - v8rec->Get(0)->IntegerValue(), + v8rec->Get(3)->IntegerValue() - v8rec->Get(1)->IntegerValue()); + } else { + JSTHROW_TYPE("Must pass rectangle to track") + } + + CMT *to = new CMT(m->mat, r); + + to->Wrap(info.This()); + info.GetReturnValue().Set(info.This()); +} + +CMT::CMT(Mat image, Rect rect){ + Mat im0_gray; + + if (image.channels() > 1) { + cvtColor(image, im0_gray, CV_BGR2GRAY); + } else { + im0_gray = image; + } + + initialize(im0_gray, rect); +} + +NAN_METHOD(CMT::cmtTrack){ + SETUP_FUNCTION(CMT) + + if (info.Length() != 1){ + Nan::ThrowTypeError("track takes an image param"); + return; + } + + Matrix *im = Nan::ObjectWrap::Unwrap(info[0]->ToObject()); + Mat im0_gray; + Mat image = im->mat; + + if (image.channels() > 1) { + cvtColor(image, im0_gray, CV_BGR2GRAY); + } else { + im0_gray = image; + } + self->processFrame(im0_gray); + + Rect r = self->bb_rot.boundingRect(); + + v8::Local arr = Nan::New(4); + + arr->Set(0, Nan::New(r.x)); + arr->Set(1, Nan::New(r.y)); + arr->Set(2, Nan::New(r.width)); + arr->Set(3, Nan::New(r.height)); + arr->Set(4, Nan::New(self->oCenter.x)); + arr->Set(5, Nan::New(self->oCenter.y)); + + info.GetReturnValue().Set(arr); +} + +void CMT::initialize(const Mat im_gray, const Rect rect) +{ + str_detector = "FAST"; + str_descriptor = "BRISK"; + + //Remember initial size + size_initial = rect.size(); + + //Remember initial image + im_prev = im_gray; + + //Compute center of rect + Point2f center = Point2f(rect.x + rect.width/2.0, rect.y + rect.height/2.0); + + //Initialize rotated bounding box + bb_rot = RotatedRect(center, size_initial, 0.0); + + //Initialize detector and descriptor +#if CV_MAJOR_VERSION > 2 + detector = cv::FastFeatureDetector::create(); + descriptor = cv::BRISK::create(); +#else + detector = FeatureDetector::create(str_detector); + descriptor = DescriptorExtractor::create(str_descriptor); +#endif + + //Get initial keypoints in whole image and compute their descriptors + vector keypoints; + detector->detect(im_gray, keypoints); + + //Divide keypoints into foreground and background keypoints according to selection + vector keypoints_fg; + vector keypoints_bg; + + for (size_t i = 0; i < keypoints.size(); i++) + { + KeyPoint k = keypoints[i]; + Point2f pt = k.pt; + + if (pt.x > rect.x && pt.y > rect.y && pt.x < rect.br().x && pt.y < rect.br().y) + { + keypoints_fg.push_back(k); + } + + else + { + keypoints_bg.push_back(k); + } + + } + + //Create foreground classes + vector classes_fg; + classes_fg.reserve(keypoints_fg.size()); + for (size_t i = 0; i < keypoints_fg.size(); i++) + { + classes_fg.push_back(i); + } + + //Compute foreground/background features + Mat descs_fg; + Mat descs_bg; + descriptor->compute(im_gray, keypoints_fg, descs_fg); + descriptor->compute(im_gray, keypoints_bg, descs_bg); + + //Only now is the right time to convert keypoints to points, as compute() might remove some keypoints + vector points_fg; + vector points_bg; + + for (size_t i = 0; i < keypoints_fg.size(); i++) + { + points_fg.push_back(keypoints_fg[i].pt); + } + + for (size_t i = 0; i < keypoints_bg.size(); i++) + { + points_bg.push_back(keypoints_bg[i].pt); + } + + //Create normalized points + vector points_normalized; + for (size_t i = 0; i < points_fg.size(); i++) + { + points_normalized.push_back(points_fg[i] - center); + } + + //Initialize matcher + matcher.initialize(points_normalized, descs_fg, classes_fg, descs_bg, center); + + //Initialize consensus + consensus.initialize(points_normalized); + + //Create initial set of active keypoints + for (size_t i = 0; i < keypoints_fg.size(); i++) + { + points_active.push_back(keypoints_fg[i].pt); + classes_active = classes_fg; + } +} + +void CMT::processFrame(Mat im_gray) { + //Track keypoints + vector points_tracked; + vector status; + tracker.track(im_prev, im_gray, points_active, points_tracked, status); + + //keep only successful classes + vector classes_tracked; + for (size_t i = 0; i < classes_active.size(); i++) + { + if (status[i]) + { + classes_tracked.push_back(classes_active[i]); + } + + } + + //Detect keypoints, compute descriptors + vector keypoints; + detector->detect(im_gray, keypoints); + + Mat descriptors; + descriptor->compute(im_gray, keypoints, descriptors); + + //Match keypoints globally + vector points_matched_global; + vector classes_matched_global; + matcher.matchGlobal(keypoints, descriptors, points_matched_global, classes_matched_global); + + //Fuse tracked and globally matched points + vector points_fused; + vector classes_fused; + fusion.preferFirst(points_tracked, classes_tracked, points_matched_global, classes_matched_global, + points_fused, classes_fused); + + //Estimate scale and rotation from the fused points + float scale; + float rotation; + consensus.estimateScaleRotation(points_fused, classes_fused, scale, rotation); + + //Find inliers and the center of their votes + Point2f center; + vector points_inlier; + vector classes_inlier; + consensus.findConsensus(points_fused, classes_fused, scale, rotation, + center, points_inlier, classes_inlier); + oCenter = center; + + //Match keypoints locally + vector points_matched_local; + vector classes_matched_local; + matcher.matchLocal(keypoints, descriptors, center, scale, rotation, points_matched_local, classes_matched_local); + + //Clear active points + points_active.clear(); + classes_active.clear(); + + //Fuse locally matched points and inliers + fusion.preferFirst(points_matched_local, classes_matched_local, points_inlier, classes_inlier, points_active, classes_active); +// points_active = points_fused; +// classes_active = classes_fused; + + //TODO: Use theta to suppress result + bb_rot = RotatedRect(center, size_initial * scale, rotation/CV_PI * 180); + + //Remember current image + im_prev = im_gray; +} diff --git a/src/CMT/CMT.h b/src/CMT/CMT.h new file mode 100644 index 00000000..6c7a9726 --- /dev/null +++ b/src/CMT/CMT.h @@ -0,0 +1,52 @@ +#include "common.h" +#include "Consensus.h" +#include "Fusion.h" +#include "Matcher.h" +#include "Tracker.h" +#include "../OpenCV.h" + +#include + +using cv::FeatureDetector; +using cv::DescriptorExtractor; +using cv::Ptr; +using cv::RotatedRect; +using cv::Size2f; + +class CMT: public Nan::ObjectWrap { +public: + static Nan::Persistent constructor; + static void Init(Local target); + static NAN_METHOD(New); + + CMT(Mat image, Rect rect); + + JSFUNC(cmtTrack); + + void initialize(const Mat im_gray, const Rect rect); + void processFrame(const Mat im_gray); + + Fusion fusion; + Matcher matcher; + Tracker tracker; + Consensus consensus; + + string str_detector; + string str_descriptor; + + vector points_active; //public for visualization purposes + RotatedRect bb_rot; + +private: + Ptr detector; + Ptr descriptor; + + Size2f size_initial; + + vector classes_active; + + float theta; + + Mat im_prev; + Point2f oCenter; +}; diff --git a/src/CMT/CMT.h~ b/src/CMT/CMT.h~ new file mode 100644 index 00000000..c1803ca4 --- /dev/null +++ b/src/CMT/CMT.h~ @@ -0,0 +1,51 @@ +#include "common.h" +#include "Consensus.h" +#include "Fusion.h" +#include "Matcher.h" +#include "Tracker.h" +#include "../OpenCV.h" + +#include + +using cv::FeatureDetector; +using cv::DescriptorExtractor; +using cv::Ptr; +using cv::RotatedRect; +using cv::Size2f; + +class CMT: public Nan::ObjectWrap { +public: + static Nan::Persistent constructor; + static void Init(Local target); + static NAN_METHOD(New); + + CMT(Mat image, Rect rect); + JSFUNC(cmtTrack); + + void initialize(const Mat im_gray, const Rect rect); + void processFrame(const Mat im_gray); + + Fusion fusion; + Matcher matcher; + Tracker tracker; + Consensus consensus; + + string str_detector; + string str_descriptor; + + vector points_active; //public for visualization purposes + RotatedRect bb_rot; + +private: + Ptr detector; + Ptr descriptor; + + Size2f size_initial; + + vector classes_active; + + float theta; + + Mat im_prev; + Point2f oCenter; +}; diff --git a/src/CMT/Consensus.cpp b/src/CMT/Consensus.cpp new file mode 100644 index 00000000..c42a5e5b --- /dev/null +++ b/src/CMT/Consensus.cpp @@ -0,0 +1,213 @@ +#include "Consensus.h" + +#define _USE_MATH_DEFINES //Necessary for M_PI to be available on Windows +#include + +#include "fastcluster/fastcluster.h" + +using std::max_element; +using std::distance; + +void Consensus::initialize(const vector & points_normalized) +{ +// FILE_LOG(logDEBUG) << "Consensus::initialize() call"; + + //Copy normalized points + this->points_normalized = points_normalized; + + size_t num_points = points_normalized.size(); + + //Create matrices of pairwise distances/angles + distances_pairwise = Mat(num_points, num_points, CV_32FC1); + angles_pairwise = Mat(num_points, num_points, CV_32FC1); + + for (size_t i = 0; i < num_points; i++) + { + for (size_t j = 0; j < num_points; j++) + { + Point2f v = points_normalized[i] - points_normalized[j]; + + float distance = norm(v); + float angle = atan2(v.y,v.x); + + distances_pairwise.at(i,j) = distance; + angles_pairwise.at(i,j) = angle; + } + + } + +// FILE_LOG(logDEBUG) << "Consensus::initialize() return"; +} + + +//TODO: Check for estimate_scale, estimate_rotation +void Consensus::estimateScaleRotation(const vector & points, const vector & classes, + float & scale, float & rotation) +{ +// FILE_LOG(logDEBUG) << "Consensus::estimateScaleRotation() call"; + + //Compute pairwise changes in scale/rotation + vector changes_scale; + if (estimate_scale) changes_scale.reserve(points.size()*points.size()); + vector changes_angles; + if (estimate_rotation) changes_angles.reserve(points.size()*points.size()); + + for (size_t i = 0; i < points.size(); i++) + { + for (size_t j = 0; j < points.size(); j++) + { + if (classes[i] != classes[j]) + { + Point2f v = points[i] - points[j]; + + if (estimate_scale) + { + float distance = norm(v); + float distance_original = distances_pairwise.at(classes[i],classes[j]); + float change_scale = distance / distance_original; + changes_scale.push_back(change_scale); + } + + if (estimate_rotation) + { + float angle = atan2(v.y,v.x); + float angle_original = angles_pairwise.at(classes[i],classes[j]); + float change_angle = angle - angle_original; + + //Fix long way angles + if (fabs(change_angle) > M_PI) { + change_angle = sgn(change_angle) * 2 * M_PI + change_angle; + } + + changes_angles.push_back(change_angle); + } + } + + } + + } + + //Do not use changes_scale, changes_angle after this point as their order is changed by median() + if (changes_scale.size() < 2) scale = 1; + else scale = median(changes_scale); + + if (changes_angles.size() < 2) rotation = 0; + else rotation = median(changes_angles); + +// FILE_LOG(logDEBUG) << "Consensus::estimateScaleRotation() return"; +} + +void Consensus::findConsensus(const vector & points, const vector & classes, + const float scale, const float rotation, + Point2f & center, vector & points_inlier, vector & classes_inlier) +{ +// FILE_LOG(logDEBUG) << "Consensus::findConsensus() call"; + + //If no points are available, reteurn nan + if (points.size() == 0) + { + center.x = numeric_limits::quiet_NaN(); + center.y = numeric_limits::quiet_NaN(); + +// FILE_LOG(logDEBUG) << "Consensus::findConsensus() return"; + + return; + } + + //Compute votes + vector votes(points.size()); + for (size_t i = 0; i < points.size(); i++) + { + votes[i] = points[i] - scale * rotate(points_normalized[classes[i]], rotation); + } + + t_index N = points.size(); + + float * D = new float[N*(N-1)/2]; //This is a lot of memory, so we put it on the heap + cluster_result Z(N-1); + + //Compute pairwise distances between votes + int index = 0; + for (size_t i = 0; i < points.size(); i++) + { + for (size_t j = i+1; j < points.size(); j++) + { + //TODO: This index calculation is correct, but is it a good thing? + //int index = i * (points.size() - 1) - (i*i + i) / 2 + j - 1; + D[index] = norm(votes[i] - votes[j]); + index++; + } + } + +// FILE_LOG(logDEBUG) << "Consensus::MST_linkage_core() call"; + MST_linkage_core(N,D,Z); +// FILE_LOG(logDEBUG) << "Consensus::MST_linkage_core() return"; + + union_find nodes(N); + + //Sort linkage by distance ascending + std::stable_sort(Z[0], Z[N-1]); + + //S are cluster sizes + int * S = new int[2*N-1]; + //TODO: Why does this loop go to 2*N-1? Shouldn't it be simply N? Everything > N gets overwritten later + for(int i = 0; i < 2*N-1; i++) + { + S[i] = 1; + } + + t_index parent = 0; //After the loop ends, parent contains the index of the last cluster + for (node const * NN=Z[0]; NN!=Z[N-1]; ++NN) + { + // Get two data points whose clusters are merged in step i. + // Find the cluster identifiers for these points. + t_index node1 = nodes.Find(NN->node1); + t_index node2 = nodes.Find(NN->node2); + + // Merge the nodes in the union-find data structure by making them + // children of a new node + // if the distance is appropriate + if (NN->dist < thr_cutoff) + { + parent = nodes.Union(node1, node2); + S[parent] = S[node1] + S[node2]; + } + } + + //Get cluster labels + int * T = new int[N]; + for (t_index i = 0; i < N; i++) + { + T[i] = nodes.Find(i); + } + + //Find largest cluster + int S_max = distance(S, max_element(S, S + 2*N-1)); + + //Find inliers, compute center of votes + points_inlier.reserve(S[S_max]); + classes_inlier.reserve(S[S_max]); + center.x = center.y = 0; + + for (size_t i = 0; i < points.size(); i++) + { + //If point is in consensus cluster + if (T[i] == S_max) + { + points_inlier.push_back(points[i]); + classes_inlier.push_back(classes[i]); + center.x += votes[i].x; + center.y += votes[i].y; + } + + } + + center.x /= points_inlier.size(); + center.y /= points_inlier.size(); + + delete[] D; + delete[] S; + delete[] T; + +// FILE_LOG(logDEBUG) << "Consensus::findConsensus() return"; +} diff --git a/src/CMT/Consensus.h b/src/CMT/Consensus.h new file mode 100644 index 00000000..de8cc37b --- /dev/null +++ b/src/CMT/Consensus.h @@ -0,0 +1,29 @@ +#ifndef CONSENSUS_H + +#define CONSENSUS_H + +#include "common.h" + +class Consensus +{ +public: + Consensus() : estimate_scale(true), estimate_rotation(false), thr_cutoff(20) {}; + + void initialize(const vector & points_normalized); + void estimateScaleRotation(const vector & points, const vector & classes, + float & scale, float & rotation); + void findConsensus(const vector & points, const vector & classes, + const float scale, const float rotation, + Point2f & center, vector & points_inlier, vector & classes_inlier); + + bool estimate_scale; + bool estimate_rotation; + +private: + float thr_cutoff; + vector points_normalized; + Mat distances_pairwise; + Mat angles_pairwise; +}; + +#endif /* end of include guard: CONSENSUS_H */ diff --git a/src/CMT/Fusion.cpp b/src/CMT/Fusion.cpp new file mode 100644 index 00000000..4c9a6ddb --- /dev/null +++ b/src/CMT/Fusion.cpp @@ -0,0 +1,32 @@ +#include "Fusion.h" + +void Fusion::preferFirst(const vector & points_first, const vector & classes_first, + const vector & points_second, const vector & classes_second, + vector & points_fused, vector & classes_fused) +{ +// FILE_LOG(logDEBUG) << "Fusion::preferFirst() call"; + + points_fused = points_first; + classes_fused = classes_first; + + for (size_t i = 0; i < points_second.size(); i++) + { + int class_second = classes_second[i]; + + bool found = false; + for (size_t j = 0; j < points_first.size(); j++) + { + int class_first = classes_first[j]; + if (class_first == class_second) found = true; + } + + if (!found) + { + points_fused.push_back(points_second[i]); + classes_fused.push_back(class_second); + } + + } + +// FILE_LOG(logDEBUG) << "Fusion::preferFirst() return"; +} diff --git a/src/CMT/Fusion.h b/src/CMT/Fusion.h new file mode 100644 index 00000000..f943ac2e --- /dev/null +++ b/src/CMT/Fusion.h @@ -0,0 +1,15 @@ +#ifndef FUSION_H + +#define FUSION_H + +#include "common.h" + +class Fusion +{ +public: + void preferFirst(const vector & firstPoints, const vector & firstClasses, + const vector & secondPoints, const vector & secondClasses, + vector & fusedPoints, vector & fusedClasses); +}; + +#endif /* end of include guard: FUSION_H */ diff --git a/src/CMT/Matcher.cpp b/src/CMT/Matcher.cpp new file mode 100644 index 00000000..6a97535d --- /dev/null +++ b/src/CMT/Matcher.cpp @@ -0,0 +1,139 @@ +#include "Matcher.h" + +using cv::vconcat; +using cv::DMatch; + +void Matcher::initialize(const vector & pts_fg_norm, const Mat desc_fg, const vector & classes_fg, + const Mat desc_bg, const Point2f center) +{ +// FILE_LOG(logDEBUG) << "Matcher::initialize() call"; + + //Copy normalized points + this->pts_fg_norm = pts_fg_norm; + + //Remember number of background points + this->num_bg_points = desc_bg.rows; + + //Form database by stacking background and foreground features + if (desc_bg.rows > 0 && desc_fg.rows > 0) + vconcat(desc_bg, desc_fg, database); + else if (desc_bg.rows > 0) + database = desc_bg; + else + database = desc_fg; + + //Extract descriptor length from features + desc_length = database.cols*8; + + //Create background classes (-1) + vector classes_bg = vector(desc_bg.rows,-1); + + //Concatenate fg and bg classes + classes = classes_bg; + classes.insert(classes.end(), classes_fg.begin(), classes_fg.end()); + + //Create descriptor matcher + bfmatcher = DescriptorMatcher::create("BruteForce-Hamming"); + +// FILE_LOG(logDEBUG) << "Matcher::initialize() return"; +} + +void Matcher::matchGlobal(const vector & keypoints, const Mat descriptors, + vector & points_matched, vector & classes_matched) +{ +// FILE_LOG(logDEBUG) << "Matcher::matchGlobal() call"; + + if (keypoints.size() == 0) + { +// FILE_LOG(logDEBUG) << "Matcher::matchGlobal() return"; + return; + } + + vector > matches; + bfmatcher->knnMatch(descriptors, database, matches, 2); + + for (size_t i = 0; i < matches.size(); i++) + { + vector m = matches[i]; + + float distance1 = m[0].distance / desc_length; + float distance2 = m[1].distance / desc_length; + int matched_class = classes[m[0].trainIdx]; + + if (matched_class == -1) continue; + if (distance1 > thr_dist) continue; + if (distance1/distance2 > thr_ratio) continue; + + points_matched.push_back(keypoints[i].pt); + classes_matched.push_back(matched_class); + } + +// FILE_LOG(logDEBUG) << "Matcher::matchGlobal() return"; +} + +void Matcher::matchLocal(const vector & keypoints, const Mat descriptors, + const Point2f center, const float scale, const float rotation, + vector & points_matched, vector & classes_matched) +{ +// FILE_LOG(logDEBUG) << "Matcher::matchLocal() call"; + + if (keypoints.size() == 0) { +// FILE_LOG(logDEBUG) << "Matcher::matchLocal() return"; + return; + } + + //Transform initial points + vector pts_fg_trans; + pts_fg_trans.reserve(pts_fg_norm.size()); + for (size_t i = 0; i < pts_fg_norm.size(); i++) + { + pts_fg_trans.push_back(scale * rotate(pts_fg_norm[i], -rotation)); + } + + //Perform local matching + for (size_t i = 0; i < keypoints.size(); i++) + { + //Normalize keypoint with respect to center + Point2f location_rel = keypoints[i].pt - center; + + //Find potential indices for matching + vector indices_potential; + for (size_t j = 0; j < pts_fg_trans.size(); j++) + { + float l2norm = norm(pts_fg_trans[j] - location_rel); + + if (l2norm < thr_cutoff) { + indices_potential.push_back(num_bg_points + j); + } + + } + + //If there are no potential matches, continue + if (indices_potential.size() == 0) continue; + + //Build descriptor matrix and classes from potential indices + Mat database_potential = Mat(indices_potential.size(), database.cols, database.type()); + for (size_t j = 0; j < indices_potential.size(); j++) { + database.row(indices_potential[j]).copyTo(database_potential.row(j)); + } + + //Find distances between descriptors + vector > matches; + bfmatcher->knnMatch(descriptors.row(i), database_potential, matches, 2); + + vector m = matches[0]; + + float distance1 = m[0].distance / desc_length; + float distance2 = m.size() > 1 ? m[1].distance / desc_length : 1; + + if (distance1 > thr_dist) continue; + if (distance1/distance2 > thr_ratio) continue; + + int matched_class = classes[indices_potential[m[0].trainIdx]]; + + points_matched.push_back(keypoints[i].pt); + classes_matched.push_back(matched_class); + } + +// FILE_LOG(logDEBUG) << "Matcher::matchLocal() return"; +} diff --git a/src/CMT/Matcher.h b/src/CMT/Matcher.h new file mode 100644 index 00000000..873479fc --- /dev/null +++ b/src/CMT/Matcher.h @@ -0,0 +1,37 @@ +#ifndef MATCHER_H + +#define MATCHER_H + +#include "common.h" + +#include "opencv2/features2d/features2d.hpp" + +using cv::KeyPoint; +using cv::Ptr; +using cv::DescriptorMatcher; + +class Matcher +{ +public: + Matcher() : thr_dist(0.25), thr_ratio(0.8), thr_cutoff(20) {}; + void initialize(const vector & pts_fg_norm, const Mat desc_fg, const vector & classes_fg, + const Mat desc_bg, const Point2f center); + void matchGlobal(const vector & keypoints, const Mat descriptors, + vector & points_matched, vector & classes_matched); + void matchLocal(const vector & keypoints, const Mat descriptors, + const Point2f center, const float scale, const float rotation, + vector & points_matched, vector & classes_matched); + +private: + vector pts_fg_norm; + Mat database; + vector classes; + int desc_length; + int num_bg_points; + Ptr bfmatcher; + float thr_dist; + float thr_ratio; + float thr_cutoff; +}; + +#endif /* end of include guard: MATCHER_H */ diff --git a/src/CMT/Tracker.cpp b/src/CMT/Tracker.cpp new file mode 100644 index 00000000..2290fb77 --- /dev/null +++ b/src/CMT/Tracker.cpp @@ -0,0 +1,44 @@ +#include + +#include "Tracker.h" + +void Tracker::track(const Mat im_prev, const Mat im_gray, const vector & points_prev, + vector & points_tracked, vector & status) +{ +// FILE_LOG(logDEBUG) << "Tracker::track() call"; + + if (points_prev.size() > 0) + { + vector err; //Needs to be float + + //Calculate forward optical flow for prev_location + calcOpticalFlowPyrLK(im_prev, im_gray, points_prev, points_tracked, status, err); + + vector points_back; + vector status_back; + vector err_back; //Needs to be float + + //Calculate backward optical flow for prev_location + calcOpticalFlowPyrLK(im_gray, im_prev, points_tracked, points_back, status_back, err_back); + + //Traverse vector backward so we can remove points on the fly + for (int i = points_prev.size()-1; i >= 0; i--) + { + float l2norm = norm(points_back[i] - points_prev[i]); + + bool fb_err_is_large = l2norm > thr_fb; + + if (fb_err_is_large || !status[i] || !status_back[i]) + { + points_tracked.erase(points_tracked.begin() + i); + + //Make sure the status flag is set to 0 + status[i] = 0; + } + + } + + } + +// FILE_LOG(logDEBUG) << "Tracker::track() return"; +} diff --git a/src/CMT/Tracker.h b/src/CMT/Tracker.h new file mode 100644 index 00000000..492af3b1 --- /dev/null +++ b/src/CMT/Tracker.h @@ -0,0 +1,18 @@ +#ifndef TRACKER_H + +#define TRACKER_H + +#include "common.h" + +class Tracker +{ +public: + Tracker() : thr_fb(30) {}; + void track(const Mat im_prev, const Mat im_gray, const vector & points_prev, + vector & points_tracked, vector & status); + +private: + float thr_fb; +}; + +#endif /* end of include guard: TRACKER_H */ diff --git a/src/CMT/common.cpp b/src/CMT/common.cpp new file mode 100644 index 00000000..15680453 --- /dev/null +++ b/src/CMT/common.cpp @@ -0,0 +1,27 @@ +#include "common.h" + +using std::nth_element; + +//TODO: Check for even/uneven number of elements +//The order of the elements of A is changed +float median(vector & A) +{ + + if (A.size() == 0) + { + return numeric_limits::quiet_NaN(); + } + + nth_element(A.begin(), A.begin() + A.size()/2, A.end()); + + return A[A.size()/2]; +} + +Point2f rotate(const Point2f v, const float angle) +{ + Point2f r; + r.x = cos(angle) * v.x - sin(angle) * v.y; + r.y = sin(angle) * v.x + cos(angle) * v.y; + + return r; +} \ No newline at end of file diff --git a/src/CMT/common.h b/src/CMT/common.h new file mode 100644 index 00000000..0385f271 --- /dev/null +++ b/src/CMT/common.h @@ -0,0 +1,28 @@ +#ifndef COMMON_H + +#define COMMON_H + +#include +#include +#include + +#include + +using cv::Mat; +using cv::Point2f; +using cv::Rect; +using cv::Size2f; +using std::numeric_limits; +using std::string; +using std::vector; + + float median(vector & A); + Point2f rotate(const Point2f v, const float angle); + template + int sgn(T x) + { + if (x >=0) return 1; + else return -1; + } + +#endif /* end of include guard: COMMON_H */ diff --git a/src/CMT/common.h~ b/src/CMT/common.h~ new file mode 100644 index 00000000..31fefd4f --- /dev/null +++ b/src/CMT/common.h~ @@ -0,0 +1,30 @@ +#ifndef COMMON_H + +#define COMMON_H + +#include +#include +#include + +#include + +#include "logging/log.h" + +using cv::Mat; +using cv::Point2f; +using cv::Rect; +using cv::Size2f; +using std::numeric_limits; +using std::string; +using std::vector; + + float median(vector & A); + Point2f rotate(const Point2f v, const float angle); + template + int sgn(T x) + { + if (x >=0) return 1; + else return -1; + } + +#endif /* end of include guard: COMMON_H */ diff --git a/src/CMT/fastcluster/fastcluster.cpp b/src/CMT/fastcluster/fastcluster.cpp new file mode 100644 index 00000000..9db531fb --- /dev/null +++ b/src/CMT/fastcluster/fastcluster.cpp @@ -0,0 +1,1504 @@ +/* + fastcluster: Fast hierarchical clustering routines for R and Python + + Copyright © 2011 Daniel Müllner + + + This library implements various fast algorithms for hierarchical, + agglomerative clustering methods: + + (1) Algorithms for the "stored matrix approach": the input is the array of + pairwise dissimilarities. + + MST_linkage_core: single linkage clustering with the "minimum spanning + tree algorithm (Rohlfs) + + NN_chain_core: nearest-neighbor-chain algorithm, suitable for single, + complete, average, weighted and Ward linkage (Murtagh) + + generic_linkage: generic algorithm, suitable for all distance update + formulas (Müllner) + + (2) Algorithms for the "stored data approach": the input are points in a + vector space. + + MST_linkage_core_vector: single linkage clustering for vector data + + generic_linkage_vector: generic algorithm for vector data, suitable for + the Ward, centroid and median methods. + + generic_linkage_vector_alternative: alternative scheme for updating the + nearest neighbors. This method seems faster than "generic_linkage_vector" + for the centroid and median methods but slower for the Ward method. + + All these implementation treat infinity values correctly. They throw an + exception if a NaN distance value occurs. +*/ + + +#include "fastcluster.h" + +class doubly_linked_list { + /* + Class for a doubly linked list. Initially, the list is the integer range + [0, size]. We provide a forward iterator and a method to delete an index + from the list. + + Typical use: for (i=L.start; L succ; + +private: + auto_array_ptr pred; + // Not necessarily private, we just do not need it in this instance. + +public: + doubly_linked_list(const t_index size) + // Initialize to the given size. + : start(0) + , succ(size+1) + , pred(size+1) + { + for (t_index i=0; i(2*N-3-(r_))*(r_)>>1)+(c_)-1] ) +// Z is an ((N-1)x4)-array +#define Z_(_r, _c) (Z[(_r)*4 + (_c)]) + +class nan_error{}; +#ifdef FE_INVALID +class fenv_error{}; +#endif + +void MST_linkage_core(const t_index N, const t_float * const D, + cluster_result & Z2) { +/* + N: integer, number of data points + D: condensed distance matrix N*(N-1)/2 + Z2: output data structure + + The basis of this algorithm is an algorithm by Rohlf: + + F. James Rohlf, Hierarchical clustering using the minimum spanning tree, + The Computer Journal, vol. 16, 1973, p. 93–95. +*/ + t_index i; + t_index idx2; + doubly_linked_list active_nodes(N); + auto_array_ptr d(N); + + t_index prev_node; + t_float min; + + // first iteration + idx2 = 1; + min = std::numeric_limits::infinity(); + for (i=1; i tmp) + d[i] = tmp; + else if (fc_isnan(tmp)) + throw (nan_error()); +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + if (d[i] < min) { + min = d[i]; + idx2 = i; + } + } + Z2.append(prev_node, idx2, min); + } +} + +/* Functions for the update of the dissimilarity array */ + +inline static void f_single( t_float * const b, const t_float a ) { + if (*b > a) *b = a; +} +inline static void f_complete( t_float * const b, const t_float a ) { + if (*b < a) *b = a; +} +inline static void f_average( t_float * const b, const t_float a, const t_float s, const t_float t) { + *b = s*a + t*(*b); + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_weighted( t_float * const b, const t_float a) { + *b = (a+*b)*.5; + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_ward( t_float * const b, const t_float a, const t_float c, const t_float s, const t_float t, const t_float v) { + *b = ( (v+s)*a - v*c + (v+t)*(*b) ) / (s+t+v); + //*b = a+(*b)-(t*a+s*(*b)+v*c)/(s+t+v); + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_centroid( t_float * const b, const t_float a, const t_float stc, const t_float s, const t_float t) { + *b = s*a - stc + t*(*b); + #ifndef FE_INVALID + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} +inline static void f_median( t_float * const b, const t_float a, const t_float c_4) { + *b = (a+(*b))*.5 - c_4; + #ifndef FE_INVALID +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*b)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + #endif +} + +template +static void NN_chain_core(const t_index N, t_float * const D, t_members * const members, cluster_result & Z2) { +/* + N: integer + D: condensed distance matrix N*(N-1)/2 + Z2: output data structure + + This is the NN-chain algorithm, described on page 86 in the following book: + + Fionn Murtagh, Multidimensional Clustering Algorithms, + Vienna, Würzburg: Physica-Verlag, 1985. +*/ + t_index i; + + auto_array_ptr NN_chain(N); + t_index NN_chain_tip = 0; + + t_index idx1, idx2; + + t_float size1, size2; + doubly_linked_list active_nodes(N); + + t_float min; + + for (t_float const * DD=D; DD!=D+(static_cast(N)*(N-1)>>1); + ++DD) { +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" +#endif + if (fc_isnan(*DD)) { + throw(nan_error()); + } +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + } + + #ifdef FE_INVALID + if (feclearexcept(FE_INVALID)) throw fenv_error(); + #endif + + for (t_index j=0; jidx2) { + t_index tmp = idx1; + idx1 = idx2; + idx2 = tmp; + } + + if (method==METHOD_METR_AVERAGE || + method==METHOD_METR_WARD) { + size1 = static_cast(members[idx1]); + size2 = static_cast(members[idx2]); + members[idx2] += members[idx1]; + } + + // Remove the smaller index from the valid indices (active_nodes). + active_nodes.remove(idx1); + + switch (method) { + case METHOD_METR_SINGLE: + /* + Single linkage. + + Characteristic: new distances are never longer than the old distances. + */ + // Update the distance matrix in the range [start, idx1). + for (i=active_nodes.start; i(members[i]); + for (i=active_nodes.start; i(members[i]) ); + // Update the distance matrix in the range (idx1, idx2). + for (; i(members[i]) ); + // Update the distance matrix in the range (idx2, N). + for (i=active_nodes.succ[idx2]; i(members[i]) ); + break; + + default: + throw std::runtime_error(std::string("Invalid method.")); + } + } + #ifdef FE_INVALID + if (fetestexcept(FE_INVALID)) throw fenv_error(); + #endif +} + +class binary_min_heap { + /* + Class for a binary min-heap. The data resides in an array A. The elements of + A are not changed but two lists I and R of indices are generated which point + to elements of A and backwards. + + The heap tree structure is + + H[2*i+1] H[2*i+2] + \ / + \ / + ≤ ≤ + \ / + \ / + H[i] + + where the children must be less or equal than their parent. Thus, H[0] + contains the minimum. The lists I and R are made such that H[i] = A[I[i]] + and R[I[i]] = i. + + This implementation is not designed to handle NaN values. + */ +private: + t_float * const A; + t_index size; + auto_array_ptr I; + auto_array_ptr R; + + // no default constructor + binary_min_heap(); + // noncopyable + binary_min_heap(binary_min_heap const &); + binary_min_heap & operator=(binary_min_heap const &); + +public: + binary_min_heap(t_float * const A_, const t_index size_) + : A(A_), size(size_), I(size), R(size) + { // Allocate memory and initialize the lists I and R to the identity. This + // does not make it a heap. Call heapify afterwards! + for (t_index i=0; i>1); idx>0; ) { + --idx; + update_geq_(idx); + } + } + + inline t_index argmin() const { + // Return the minimal element. + return I[0]; + } + + void heap_pop() { + // Remove the minimal element from the heap. + --size; + I[0] = I[size]; + R[I[0]] = 0; + update_geq_(0); + } + + void remove(t_index idx) { + // Remove an element from the heap. + --size; + R[I[size]] = R[idx]; + I[R[idx]] = I[size]; + if ( H(size)<=A[idx] ) { + update_leq_(R[idx]); + } + else { + update_geq_(R[idx]); + } + } + + void replace ( const t_index idxold, const t_index idxnew, + const t_float val) { + R[idxnew] = R[idxold]; + I[R[idxnew]] = idxnew; + if (val<=A[idxold]) + update_leq(idxnew, val); + else + update_geq(idxnew, val); + } + + void update ( const t_index idx, const t_float val ) const { + // Update the element A[i] with val and re-arrange the indices to preserve + // the heap condition. + if (val<=A[idx]) + update_leq(idx, val); + else + update_geq(idx, val); + } + + void update_leq ( const t_index idx, const t_float val ) const { + // Use this when the new value is not more than the old value. + A[idx] = val; + update_leq_(R[idx]); + } + + void update_geq ( const t_index idx, const t_float val ) const { + // Use this when the new value is not less than the old value. + A[idx] = val; + update_geq_(R[idx]); + } + +private: + void update_leq_ (t_index i) const { + t_index j; + for ( ; (i>0) && ( H(i)>1) ); i=j) + heap_swap(i,j); + } + + void update_geq_ (t_index i) const { + t_index j; + for ( ; (j=2*i+1)=H(i) ) { + ++j; + if ( j>=size || H(j)>=H(i) ) break; + } + else if ( j+1 +static void generic_linkage(const t_index N, t_float * const D, t_members * const members, cluster_result & Z2) { + /* + N: integer, number of data points + D: condensed distance matrix N*(N-1)/2 + Z2: output data structure + */ + + const t_index N_1 = N-1; + t_index i, j; // loop variables + t_index idx1, idx2; // row and column indices + + auto_array_ptr n_nghbr(N_1); // array of nearest neighbors + auto_array_ptr mindist(N_1); // distances to the nearest neighbors + auto_array_ptr row_repr(N); // row_repr[i]: node number that the + // i-th row represents + doubly_linked_list active_nodes(N); + binary_min_heap nn_distances(&*mindist, N_1); // minimum heap structure for + // the distance to the nearest neighbor of each point + t_index node1, node2; // node numbers in the output + t_float size1, size2; // and their cardinalities + + t_float min; // minimum and row index for nearest-neighbor search + t_index idx; + + for (i=0; ii} D(i,j) for i in range(N-1) + t_float const * DD = D; + for (i=0; i::infinity(); + for (idx=j=i+1; ji} D(i,j) + + Normally, we have equality. However, this minimum may become invalid due + to the updates in the distance matrix. The rules are: + + 1) If mindist[i] is equal to D(i, n_nghbr[i]), this is the correct + minimum and n_nghbr[i] is a nearest neighbor. + + 2) If mindist[i] is smaller than D(i, n_nghbr[i]), this might not be the + correct minimum. The minimum needs to be recomputed. + + 3) mindist[i] is never bigger than the true minimum. Hence, we never + miss the true minimum if we take the smallest mindist entry, + re-compute the value if necessary (thus maybe increasing it) and + looking for the now smallest mindist entry until a valid minimal + entry is found. This step is done in the lines below. + + The update process for D below takes care that these rules are + fulfilled. This makes sure that the minima in the rows D(i,i+1:)of D are + re-calculated when necessary but re-calculation is avoided whenever + possible. + + The re-calculation of the minima makes the worst-case runtime of this + algorithm cubic in N. We avoid this whenever possible, and in most cases + the runtime appears to be quadratic. + */ + idx1 = nn_distances.argmin(); + if (method != METHOD_METR_SINGLE) { + while ( mindist[idx1] < D_(idx1, n_nghbr[idx1]) ) { + // Recompute the minimum mindist[idx1] and n_nghbr[idx1]. + n_nghbr[idx1] = j = active_nodes.succ[idx1]; // exists, maximally N-1 + min = D_(idx1,j); + for (j=active_nodes.succ[j]; j(members[idx1]); + size2 = static_cast(members[idx2]); + members[idx2] += members[idx1]; + } + Z2.append(node1, node2, mindist[idx1]); + + // Remove idx1 from the list of active indices (active_nodes). + active_nodes.remove(idx1); + // Index idx2 now represents the new (merged) node with label N+i. + row_repr[idx2] = N+i; + + // Update the distance matrix + switch (method) { + case METHOD_METR_SINGLE: + /* + Single linkage. + + Characteristic: new distances are never longer than the old distances. + */ + // Update the distance matrix in the range [start, idx1). + for (j=active_nodes.start; j(members[j]) ); + if (n_nghbr[j] == idx1) + n_nghbr[j] = idx2; + } + // Update the distance matrix in the range (idx1, idx2). + for (; j(members[j]) ); + if (D_(j, idx2) < mindist[j]) { + nn_distances.update_leq(j, D_(j, idx2)); + n_nghbr[j] = idx2; + } + } + // Update the distance matrix in the range (idx2, N). + if (idx2(members[j]) ); + min = D_(idx2,j); + for (j=active_nodes.succ[j]; j(members[j]) ); + if (D_(idx2,j) < min) { + min = D_(idx2,j); + n_nghbr[idx2] = j; + } + } + nn_distances.update(idx2, min); + } + break; + + case METHOD_METR_CENTROID: { + /* + Centroid linkage. + + Shorter and longer distances can occur, not bigger than max(d1,d2) + but maybe smaller than min(d1,d2). + */ + // Update the distance matrix in the range [start, idx1). + t_float s = size1/(size1+size2); + t_float t = size2/(size1+size2); + t_float stc = s*t*mindist[idx1]; + for (j=active_nodes.start; j +static void MST_linkage_core_vector(const t_index N, + t_dissimilarity & dist, + cluster_result & Z2) { +/* + N: integer, number of data points + dist: function pointer to the metric + Z2: output data structure + + The basis of this algorithm is an algorithm by Rohlf: + + F. James Rohlf, Hierarchical clustering using the minimum spanning tree, + The Computer Journal, vol. 16, 1973, p. 93–95. +*/ + t_index i; + t_index idx2; + doubly_linked_list active_nodes(N); + auto_array_ptr d(N); + + t_index prev_node; + t_float min; + + // first iteration + idx2 = 1; + min = std::numeric_limits::infinity(); + for (i=1; i tmp) + d[i] = tmp; + else if (fc_isnan(tmp)) + throw (nan_error()); +#if HAVE_DIAGNOSTIC +#pragma GCC diagnostic pop +#endif + if (d[i] < min) { + min = d[i]; + idx2 = i; + } + } + Z2.append(prev_node, idx2, min); + } +} + +template +static void generic_linkage_vector(const t_index N, + t_dissimilarity & dist, + cluster_result & Z2) { + /* + N: integer, number of data points + dist: function pointer to the metric + Z2: output data structure + + This algorithm is valid for the distance update methods + "Ward", "centroid" and "median" only! + */ + const t_index N_1 = N-1; + t_index i, j; // loop variables + t_index idx1, idx2; // row and column indices + + auto_array_ptr n_nghbr(N_1); // array of nearest neighbors + auto_array_ptr mindist(N_1); // distances to the nearest neighbors + auto_array_ptr row_repr(N); // row_repr[i]: node number that the + // i-th row represents + doubly_linked_list active_nodes(N); + binary_min_heap nn_distances(&*mindist, N_1); // minimum heap structure for + // the distance to the nearest neighbor of each point + t_index node1, node2; // node numbers in the output + t_float min; // minimum and row index for nearest-neighbor search + + for (i=0; ii} D(i,j) for i in range(N-1) + for (i=0; i::infinity(); + t_index idx; + for (idx=j=i+1; j(i,j); + } + if (tmp(idx1,j); + for (j=active_nodes.succ[j]; j(idx1,j); + if (tmp(j, idx2); + if (tmp < mindist[j]) { + nn_distances.update_leq(j, tmp); + n_nghbr[j] = idx2; + } + else if (n_nghbr[j] == idx2) + n_nghbr[j] = idx1; // invalidate + } + // Find the nearest neighbor for idx2. + if (idx2(idx2,j); + for (j=active_nodes.succ[j]; j(idx2, j); + if (tmp < min) { + min = tmp; + n_nghbr[idx2] = j; + } + } + nn_distances.update(idx2, min); + } + } + } +} + +template +static void generic_linkage_vector_alternative(const t_index N, + t_dissimilarity & dist, + cluster_result & Z2) { + /* + N: integer, number of data points + dist: function pointer to the metric + Z2: output data structure + + This algorithm is valid for the distance update methods + "Ward", "centroid" and "median" only! + */ + const t_index N_1 = N-1; + t_index i, j=0; // loop variables + t_index idx1, idx2; // row and column indices + + auto_array_ptr n_nghbr(2*N-2); // array of nearest neighbors + auto_array_ptr mindist(2*N-2); // distances to the nearest neighbors + + doubly_linked_list active_nodes(N+N_1); + binary_min_heap nn_distances(&*mindist, N_1, 2*N-2, 1); // minimum heap + // structure for the distance to the nearest neighbor of each point + + t_float min; // minimum for nearest-neighbor searches + + // Initialize the minimal distances: + // Find the nearest neighbor of each point. + // n_nghbr[i] = argmin_{j>i} D(i,j) for i in range(N-1) + for (i=1; i::infinity(); + t_index idx; + for (idx=j=0; j(i,j); + } + if (tmp // for std::ptrdiff_t +#include // for std::numeric_limits<...>::infinity() +#include // for std::fill_n +#include // for std::runtime_error +#include // for std::string +#include // for std::string + +#include "../common.h" + +#define fc_isnan(X) ((X)!=(X)) + +// Microsoft Visual Studio does not have fenv.h +#ifdef _MSC_VER +#if (_MSC_VER == 1500 || _MSC_VER == 1600) +#define NO_INCLUDE_FENV +#endif +#endif +#ifndef NO_INCLUDE_FENV +#include +#endif + +#include // also for DBL_MAX, DBL_MIN +#ifndef DBL_MANT_DIG +#error The constant DBL_MANT_DIG could not be defined. +#endif +#define T_FLOAT_MANT_DIG DBL_MANT_DIG + +#ifndef LONG_MAX +#include +#endif +#ifndef LONG_MAX +#error The constant LONG_MAX could not be defined. +#endif +#ifndef INT_MAX +#error The constant INT_MAX could not be defined. +#endif + +#ifndef INT32_MAX +#define __STDC_LIMIT_MACROS +#include +#endif + +#ifndef HAVE_DIAGNOSTIC +#if __GNUC__ > 4 || (__GNUC__ == 4 && (__GNUC_MINOR__ >= 6)) +#define HAVE_DIAGNOSTIC 1 +#endif +#endif + +#ifndef HAVE_VISIBILITY +#if __GNUC__ >= 4 +#define HAVE_VISIBILITY 1 +#endif +#endif + +/* Since the public interface is given by the Python respectively R interface, + * we do not want other symbols than the interface initalization routines to be + * visible in the shared object file. The "visibility" switch is a GCC concept. + * Hiding symbols keeps the relocation table small and decreases startup time. + * See http://gcc.gnu.org/wiki/Visibility + */ +#if HAVE_VISIBILITY +#pragma GCC visibility push(hidden) +#endif + +typedef int_fast32_t t_index; +#ifndef INT32_MAX +#define MAX_INDEX 0x7fffffffL +#else +#define MAX_INDEX INT32_MAX +#endif +#if (LONG_MAX < MAX_INDEX) +#error The integer format "t_index" must not have a greater range than "long int". +#endif +#if (INT_MAX > MAX_INDEX) +#error The integer format "int" must not have a greater range than "t_index". +#endif +typedef float t_float; + +/* Method codes. + + These codes must agree with the METHODS array in fastcluster.R and the + dictionary mthidx in fastcluster.py. +*/ +enum method_codes { + // non-Euclidean methods + METHOD_METR_SINGLE = 0, + METHOD_METR_COMPLETE = 1, + METHOD_METR_AVERAGE = 2, + METHOD_METR_WEIGHTED = 3, + METHOD_METR_WARD = 4, + METHOD_METR_WARD_D = METHOD_METR_WARD, + METHOD_METR_CENTROID = 5, + METHOD_METR_MEDIAN = 6, + METHOD_METR_WARD_D2 = 7, + + MIN_METHOD_CODE = 0, + MAX_METHOD_CODE = 7 +}; + +enum method_codes_vector { + // Euclidean methods + METHOD_VECTOR_SINGLE = 0, + METHOD_VECTOR_WARD = 1, + METHOD_VECTOR_CENTROID = 2, + METHOD_VECTOR_MEDIAN = 3, + + MIN_METHOD_VECTOR_CODE = 0, + MAX_METHOD_VECTOR_CODE = 3 +}; + +enum { + // Return values + RET_SUCCESS = 0, + RET_MEMORY_ERROR = 1, + RET_STL_ERROR = 2, + RET_UNKNOWN_ERROR = 3 + }; + +// self-destructing array pointer +template +class auto_array_ptr{ +private: + type * ptr; + auto_array_ptr(auto_array_ptr const &); // non construction-copyable + auto_array_ptr& operator=(auto_array_ptr const &); // non copyable +public: + auto_array_ptr() + : ptr(NULL) + { } + template + auto_array_ptr(index const size) + : ptr(new type[size]) + { } + template + auto_array_ptr(index const size, value const val) + : ptr(new type[size]) + { + std::fill_n(ptr, size, val); + } + ~auto_array_ptr() { + delete [] ptr; } + void free() { + delete [] ptr; + ptr = NULL; + } + template + void init(index const size) { + ptr = new type [size]; + } + template + void init(index const size, value const val) { + init(size); + std::fill_n(ptr, size, val); + } + inline operator type *() const { return ptr; } +}; + +struct node { + t_index node1, node2; + t_float dist; + + /* + inline bool operator< (const node a) const { + return this->dist < a.dist; + } + */ + + inline friend bool operator< (const node a, const node b) { + return (a.dist < b.dist); + } +}; + +class cluster_result { +private: + auto_array_ptr Z; + t_index pos; + +public: + cluster_result(const t_index size) + : Z(size) + , pos(0) + {} + + void append(const t_index node1, const t_index node2, const t_float dist) { + Z[pos].node1 = node1; + Z[pos].node2 = node2; + Z[pos].dist = dist; + ++pos; + } + + node * operator[] (const t_index idx) const { return Z + idx; } + + /* Define several methods to postprocess the distances. All these functions + are monotone, so they do not change the sorted order of distances. */ + + void sqrt() const { + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist = ::sqrt(ZZ->dist); + } + } + + void sqrt(const t_float) const { // ignore the argument + sqrt(); + } + + void sqrtdouble(const t_float) const { // ignore the argument + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist = ::sqrt(2*ZZ->dist); + } + } + + #ifdef R_pow + #define my_pow R_pow + #else + #define my_pow pow + #endif + + void power(const t_float p) const { + t_float const q = 1/p; + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist = my_pow(ZZ->dist,q); + } + } + + void plusone(const t_float) const { // ignore the argument + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist += 1; + } + } + + void divide(const t_float denom) const { + for (node * ZZ=Z; ZZ!=Z+pos; ++ZZ) { + ZZ->dist /= denom; + } + } +}; + +/* + Lookup function for a union-find data structure. + + The function finds the root of idx by going iteratively through all + parent elements until a root is found. An element i is a root if + nodes[i] is zero. To make subsequent searches faster, the entry for + idx and all its parents is updated with the root element. + */ +class union_find { +private: + auto_array_ptr parent; + t_index nextparent; + +public: + union_find(const t_index size) + : parent(size>0 ? 2*size-1 : 0, 0) + , nextparent(size) + { } + + t_index Find (t_index idx) const { + if (parent[idx] != 0 ) { // a → b + t_index p = idx; + idx = parent[idx]; + if (parent[idx] != 0 ) { // a → b → c + do { + idx = parent[idx]; + } while (parent[idx] != 0); + do { + t_index tmp = parent[p]; + parent[p] = idx; + p = tmp; + } while (parent[p] != idx); + } + } + return idx; + } + + t_index Union (const t_index node1, const t_index node2) { + parent[node1] = parent[node2] = nextparent++; + return parent[node1]; + } +}; + +void MST_linkage_core(const t_index N, const t_float * const D, + cluster_result & Z2); diff --git a/src/init.cc b/src/init.cc index a3af3bea..89f3ad14 100755 --- a/src/init.cc +++ b/src/init.cc @@ -15,6 +15,7 @@ #include "Stereo.h" #include "BackgroundSubtractor.h" #include "LDAWrap.h" +#include "CMT/CMT.h" extern "C" void init(Local target) { Nan::HandleScope scope; @@ -33,6 +34,7 @@ extern "C" void init(Local target) { StereoBM::Init(target); StereoSGBM::Init(target); StereoGC::Init(target); + CMT::Init(target); #if CV_MAJOR_VERSION >= 2 && CV_MINOR_VERSION >=4 BackgroundSubtractorWrap::Init(target); diff --git a/src/init.cc~ b/src/init.cc~ new file mode 100755 index 00000000..8dc0130a --- /dev/null +++ b/src/init.cc~ @@ -0,0 +1,46 @@ +#include "OpenCV.h" + +#include "Point.h" +#include "Matrix.h" +#include "CascadeClassifierWrap.h" +#include "VideoCaptureWrap.h" +#include "Contours.h" +#include "CamShift.h" +#include "HighGUI.h" +#include "FaceRecognizer.h" +#include "Features2d.h" +#include "Constants.h" +#include "Calib3D.h" +#include "ImgProc.h" +#include "Stereo.h" +#include "BackgroundSubtractor.h" +#include "LDAWrap.h" +#include "CMT/CMT.h" + +extern "C" void init(Local target) { + Nan::HandleScope scope; + OpenCV::Init(target); + + Point::Init(target); + Matrix::Init(target); + CascadeClassifierWrap::Init(target); + VideoCaptureWrap::Init(target); + Contour::Init(target); + TrackedObject::Init(target); + NamedWindow::Init(target); + Constants::Init(target); + Calib3D::Init(target); + ImgProc::Init(target); + StereoBM::Init(target); + StereoSGBM::Init(target); + StereoGC::Init(target); + +#if CV_MAJOR_VERSION >= 2 && CV_MINOR_VERSION >=4 + BackgroundSubtractorWrap::Init(target); + Features::Init(target); + FaceRecognizerWrap::Init(target); + LDAWrap::Init(target); +#endif +}; + +NODE_MODULE(opencv, init)