void wangpc_wdc_device::wangpcbus_aiowc_w(address_space &space, offs_t offset, UINT16 mem_mask, UINT16 data)
{
if (sad(offset) && ACCESSING_BITS_0_7)
{
switch (offset & 0x7f)
{
case 0x02/2:
// TODO command register
break;
case 0xfc/2:
device_reset();
break;
case 0xfe/2:
{
bool irq = (m_irq == ASSERT_LINE);
bool changed = ((m_option & 0x0e) != (data & 0x0e));
if (irq && changed) set_irq(CLEAR_LINE);
m_option = data & 0xff;
if (irq && changed) set_irq(ASSERT_LINE);
}
break;
}
}
}
House1() {
fileLeft = "/apps/workspaces/kiste_data/stereo/4/left.png";
fileRight = "/apps/workspaces/kiste_data/stereo/4/right.png";
imgLeft = ImageFactory::readPNG(fileLeft);
imgRight = ImageFactory::readPNG(fileRight);
MatchingSAD sad(imgLeft, imgRight, 15);
Matching matcher;
Point2i pl1(110,244); Point2i pr1(198,296); //pr1 = matcher.refine(sad, pl1, pr1);
Point2i pl2(273,242); Point2i pr2(309,249); //pr2 = matcher.refine(sad, pl2, pr2);
Point2i pl3(88,117); Point2i pr3(129,121); //pr3 = matcher.refine(sad, pl3, pr3);
Point2i pl4(247,122); Point2i pr4(244,89); //pr4 = matcher.refine(sad, pl4, pr4);
Point2i pl5(321,100); Point2i pr5(320,50); //pr5 = matcher.refine(sad, pl5, pr5);
Point2i pl6(398,162); Point2i pr6(428,125); //pr6 = matcher.refine(sad, pl6, pr6);
Point2i pl7(393,282); Point2i pr7(424,273); //pr7 = matcher.refine(sad, pl7, pr7);
Point2i pl8(266,154); Point2i pr8(278,132); //pr8 = matcher.refine(sad, pl8, pr8);
fm.addCorrespondence(pl1, pr1);
fm.addCorrespondence(pl2, pr2);
fm.addCorrespondence(pl3, pr3);
fm.addCorrespondence(pl4, pr4);
fm.addCorrespondence(pl5, pr5);
fm.addCorrespondence(pl6, pr6);
fm.addCorrespondence(pl7, pr7);
fm.addCorrespondence(pl8, pr8);
//fm.addCorrespondence(pl9, pr9);
fm.estimate();
}
void wangpc_emb_device::wangpcbus_aiowc_w(address_space &space, offs_t offset, UINT16 mem_mask, UINT16 data)
{
if (sad(offset))
{
logerror("emb write %06x:%02x\n", offset*2, data);
switch (offset & 0x7f)
{
case 0xc0/2:
m_option = data;
break;
case 0xce/2:
m_parity_error = 0;
break;
case 0xfc/2:
device_reset();
break;
case 0xfe/2:
m_parity_odd = BIT(data, 9);
break;
}
}
}
Metal1() {
fileLeft = "/apps/workspaces/kiste_data/stereo/3/left.png";
fileRight = "/apps/workspaces/kiste_data/stereo/3/right.png";
imgLeft = ImageFactory::readPNG(fileLeft);
imgRight = ImageFactory::readPNG(fileRight);
MatchingSAD sad(imgLeft, imgRight, 15);
Matching matcher;
Point2i pl1(608,834); Point2i pr1(400,873); //pr1 = matcher.refine(sad, pl1, pr1);
Point2i pl2(357,485); Point2i pr2(292,470); //pr2 = matcher.refine(sad, pl2, pr2);
Point2i pl3(461,672); Point2i pr3(294,678); //pr3 = matcher.refine(sad, pl3, pr3);
Point2i pl4(524,507); Point2i pr4(410,523); //pr4 = matcher.refine(sad, pl4, pr4);
Point2i pl5(518,479); Point2i pr5(440,493); //pr5 = matcher.refine(sad, pl5, pr5);
Point2i pl6(586,405); Point2i pr6(476,431); //pr6 = matcher.refine(sad, pl6, pr6);
Point2i pl7(593,324); Point2i pr7(532,349); //pr7 = matcher.refine(sad, pl7, pr7);
Point2i pl8(673,366); Point2i pr8(596,405); //pr8 = matcher.refine(sad, pl8, pr8);
fm.addCorrespondence(pl1, pr1);
fm.addCorrespondence(pl2, pr2);
fm.addCorrespondence(pl3, pr3);
fm.addCorrespondence(pl4, pr4);
fm.addCorrespondence(pl5, pr5);
fm.addCorrespondence(pl6, pr6);
fm.addCorrespondence(pl7, pr7);
fm.addCorrespondence(pl8, pr8);
//fm.addCorrespondence(pl9, pr9);
fm.estimate();
}
UINT16 wangpc_wdc_device::wangpcbus_iorc_r(address_space &space, offs_t offset, UINT16 mem_mask)
{
UINT16 data = 0xffff;
if (sad(offset))
{
switch (offset & 0x7f)
{
case 0x00/2:
data = m_status;
break;
case 0x02/2:
// TODO operation status register
break;
case 0x04/2:
set_irq(CLEAR_LINE);
break;
case 0xfe/2:
data = 0xff00 | (m_irq << 7) | OPTION_ID;
break;
}
}
return data;
}
/**
Just SOME kind of testing, nothing meaningful.
This test is just company to the Qt4Qt5TestModule,
which is mean to demostrate how to implement a
module for both Qt4 and Qt5
*/
int QWonderStringTest(int /*argc*/, char* /*argv*/[])
{
MITK_TEST_CONDITION( !QWonderString::penguinUID().isEmpty(), "Penguin registry working" )
QWonderString s;
QSignalSpy happyCounter( &s, SIGNAL(happy()));
QSignalSpy sadCounter( &s, SIGNAL(sad()));
MITK_TEST_CONDITION( happyCounter.count() == 0 && sadCounter.count() == 0, "No feelings in the beginning" )
s.setText("Happy feet");
MITK_TEST_CONDITION( happyCounter.count() == 1, "Penguins are happy" )
s.setText("Talk too much");
MITK_TEST_CONDITION( sadCounter.count() == 1, "Penguin ears bleed" )
s.setText("Fishy fish");
MITK_TEST_CONDITION( happyCounter.count() == 2, "Penguins sleepy" )
s.setText("Knok knok");
MITK_TEST_CONDITION( sadCounter.count() == 2, "Penguins irritated" )
return EXIT_SUCCESS;
}
UINT16 wangpc_tig_device::wangpcbus_iorc_r(address_space &space, offs_t offset, UINT16 mem_mask)
{
UINT16 data = 0xffff;
if (sad(offset))
{
switch (offset & 0x7f)
{
case 0x20/2:
case 0x22/2:
data = m_hgdc0->read(space, offset);
break;
case 0x24/2:
case 0x26/2:
data = m_hgdc1->read(space, offset);
break;
case 0xfe/2:
data = 0xff00 | (DMA_ID ? OPTION_ID_1 : OPTION_ID_0);
break;
}
}
return data;
}
IDIS() {
fileLeft = "/apps/workspaces/kiste_data/stereo/2/left.png";
fileRight = "/apps/workspaces/kiste_data/stereo/2/right.png";
imgLeft = ImageFactory::readPNG(fileLeft);
imgRight = ImageFactory::readPNG(fileRight);
MatchingSAD sad(imgLeft, imgRight, 15);
Matching matcher;
Point2i pl1(257,369); Point2i pr1(98,367); pr1 = matcher.refine(sad, pl1, pr1);
Point2i pl2(261,579); Point2i pr2(101,589); pr2 = matcher.refine(sad, pl2, pr2);
Point2i pl3(282,692); Point2i pr3(121,708); pr3 = matcher.refine(sad, pl3, pr3);
Point2i pl4(657,673); Point2i pr4(520,678); pr4 = matcher.refine(sad, pl4, pr4);
Point2i pl5(704,247); Point2i pr5(569,247); pr5 = matcher.refine(sad, pl5, pr5);
Point2i pl6(816,641); Point2i pr6(679,641); pr6 = matcher.refine(sad, pl6, pr6);
Point2i pl7(1113,503); Point2i pr7(964,501); pr7 = matcher.refine(sad, pl7, pr7);
Point2i pl8(1025,472); Point2i pr8(882,472); pr8 = matcher.refine(sad, pl8, pr8);
Point2i pl9(1096,345); Point2i pr9(948,348); pr9 = matcher.refine(sad, pl9, pr9);
fm.addCorrespondence(pl1, pr1);
fm.addCorrespondence(pl2, pr2);
fm.addCorrespondence(pl3, pr3);
fm.addCorrespondence(pl4, pr4);
fm.addCorrespondence(pl5, pr5);
fm.addCorrespondence(pl6, pr6);
fm.addCorrespondence(pl7, pr7);
fm.addCorrespondence(pl8, pr8);
//fm.addCorrespondence(pl9, pr9);
fm.estimate();
}
House3D() {
fileLeft = "/apps/workspaces/kiste_data/stereo/5/left.png";
fileRight = "/apps/workspaces/kiste_data/stereo/5/right.png";
imgLeft = ImageFactory::readPNG(fileLeft);
imgRight = ImageFactory::readPNG(fileRight);
// imgLeft = Derivative::getX(imgLeft);
// imgRight = Derivative::getX(imgRight);
// Normalize::inplace(imgLeft);
// Normalize::inplace(imgRight);
MatchingSAD sad(imgLeft, imgRight, 15);
Matching matcher;
Point2i pl1(245,381); Point2i pr1(176,366); //pr1 = matcher.refine(sad, pl1, pr1); // h1 lower left
Point2i pl2(308,323); Point2i pr2(312,323); //pr2 = matcher.refine(sad, pl2, pr2); // h1 lower right
Point2i pl3(222,82); Point2i pr3(246,82); //pr3 = matcher.refine(sad, pl3, pr3); // h1 upper right
Point2i pl4(134,76); Point2i pr4(101,78); //pr4 = matcher.refine(sad, pl4, pr4); // h1 upper left
Point2i pl5(501,218); Point2i pr5(416,228); //pr5 = matcher.refine(sad, pl5, pr5); // h1 mid left
Point2i pl6(620,201); Point2i pr6(625,219); //pr6 = matcher.refine(sad, pl6, pr6); // h1 mid right
Point2i pl7(546,83); Point2i pr7(570,78); //pr7 = matcher.refine(sad, pl7, pr7); // h1 upper right
Point2i pl8(423,79); Point2i pr8(378,77); //pr8 = matcher.refine(sad, pl8, pr8); // h1 upper left
Point2i pl9(381,413); Point2i pr9(319,420); //pr8 = matcher.refine(sad, pl8, pr8); // cube1a
Point2i plA(153,322); Point2i prA(68,299); //pr8 = matcher.refine(sad, pl8, pr8); // cube2
Point2i plB(348,361); Point2i prB(294,362); //pr8 = matcher.refine(sad, pl8, pr8); // cube1b
// Point2i pl1(242,219); Point2i pr1(171,212); //pr1 = matcher.refine(sad, pl1, pr1); // h1 lower left
// Point2i pl2(307,194); Point2i pr2(311,194); //pr2 = matcher.refine(sad, pl2, pr2); // h1 lower right
// Point2i pl3(222,82); Point2i pr3(246,82); //pr3 = matcher.refine(sad, pl3, pr3); // h1 upper right
// Point2i pl4(134,76); Point2i pr4(101,78); //pr4 = matcher.refine(sad, pl4, pr4); // h1 upper left
// Point2i pl5(501,218); Point2i pr5(416,228); //pr5 = matcher.refine(sad, pl5, pr5); // h1 lower left
// Point2i pl6(620,201); Point2i pr6(625,219); //pr6 = matcher.refine(sad, pl6, pr6); // h1 lower right
// Point2i pl7(546,83); Point2i pr7(570,78); //pr7 = matcher.refine(sad, pl7, pr7); // h1 upper right
// Point2i pl8(423,79); Point2i pr8(378,77); //pr8 = matcher.refine(sad, pl8, pr8); // h1 upper left
fm.addCorrespondence(pl1, pr1);
fm.addCorrespondence(pl2, pr2);
fm.addCorrespondence(pl3, pr3);
fm.addCorrespondence(pl4, pr4);
fm.addCorrespondence(pl5, pr5);
fm.addCorrespondence(pl6, pr6);
fm.addCorrespondence(pl7, pr7);
fm.addCorrespondence(pl8, pr8);
fm.addCorrespondence(pl9, pr9);
fm.addCorrespondence(plA, prA);
fm.addCorrespondence(plB, prB);
fm.estimate();
}
void wangpc_lic_device::wangpcbus_aiowc_w(offs_t offset, uint16_t mem_mask, uint16_t data)
{
if (sad(offset))
{
switch (offset & 0x7f)
{
case 0xfc/2:
device_reset();
break;
}
}
}
void wangpc_rtc_device::wangpcbus_aiowc_w(address_space &space, offs_t offset, UINT16 mem_mask, UINT16 data)
{
if (sad(offset))
{
switch (offset & 0x7f)
{
case 0xfc/2:
device_reset();
break;
}
}
}
main()
{
int best_block_x, best_block_y;
unsigned int min_diff;
int iterations = 2;
printf("PSADBW Example\n--------------\n\n");
printf("Executing 'motion estimation' %d times...\n\n", iterations);
min_diff = sad(0, 0, &best_block_x, &best_block_y, iterations);
printf("MinDiff: %u\nBest X: %d\nBest Y: %d\n", min_diff, best_block_x, best_block_y);
return 0;
}
void test_querystring()
{
std::string happy("foo=bar&baz=%20");
std::map<std::string, std::string> m;
int n = querystring(happy, m);
assert_equal(n, 2);
assert_equal(m.size(), 2);
assert_equal(m["foo"], std::string("bar"));
assert_equal(m["baz"], std::string(" "));
std::string sad("&");
std::map<std::string, std::string> l;
n = querystring(sad, l);
assert_equal(n, 0);
assert_equal(l.size(), 0);
}
uint16_t wangpc_lic_device::wangpcbus_iorc_r(offs_t offset, uint16_t mem_mask)
{
uint16_t data = 0xffff;
if (sad(offset))
{
switch (offset & 0x7f)
{
case 0xfe/2:
data = 0xff00 | OPTION_ID;
break;
}
}
return data;
}
UINT16 wangpc_rtc_device::wangpcbus_iorc_r(address_space &space, offs_t offset, UINT16 mem_mask)
{
UINT16 data = 0xffff;
if (sad(offset))
{
switch (offset & 0x7f)
{
case 0xfe/2:
data = 0xff00 | OPTION_ID;
break;
}
}
return data;
}
UINT16 wangpc_mvc_device::wangpcbus_iorc_r(address_space &space, offs_t offset, UINT16 mem_mask)
{
UINT16 data = 0xffff;
if (sad(offset))
{
switch (offset & 0x7f)
{
case 0xfe/2:
data = 0xff00 | (m_irq << 7) | OPTION_ID;
set_irq(CLEAR_LINE);
break;
}
}
return data;
}
void wangpc_tig_device::wangpcbus_aiowc_w(address_space &space, offs_t offset, UINT16 mem_mask, UINT16 data)
{
if (sad(offset) && ACCESSING_BITS_0_7)
{
switch (offset & 0x7f)
{
case 0x00/2: case 0x02/2: case 0x04/2: case 0x06/2: case 0x08/2: case 0x0a/2: case 0x0c/2: case 0x0e/2:
case 0x10/2: case 0x12/2: case 0x14/2: case 0x16/2: case 0x18/2: case 0x1a/2: case 0x1c/2: case 0x1e/2:
if (LOG) logerror("TIG attribute %u: %02x\n", offset, data & 0xff);
m_attr[offset] = data & 0xff;
break;
case 0x20/2:
case 0x22/2:
m_hgdc0->write(space, offset, data);
break;
case 0x24/2:
case 0x26/2:
m_hgdc1->write(space, offset, data);
break;
case 0x28/2:
if (LOG) logerror("TIG underline %02x\n", data & 0xff);
m_underline = data & 0xff;
break;
case 0x2a/2:
if (LOG) logerror("TIG option %02x\n", data & 0xff);
m_option = data & 0xff;
break;
case 0xfc/2:
device_reset();
break;
}
}
}
Garden() {
fileLeft = getDataFile("stereo1.jpg"); // left image
fileRight = getDataFile("stereo2.jpg"); // right image
imgLeft = ImageFactory::readJPEG(fileLeft);
imgRight = ImageFactory::readJPEG(fileRight);
MatchingSAD sad(imgLeft, imgRight, 15);
Matching matcher;
// image1 // image2 (left of image1) // refine the approximate matching positions
Point2i pl1(85,53); Point2i pr1(29,57); pr1 = matcher.refine(sad, pl1, pr1);
Point2i pl2(264,34); Point2i pr2(209,36); pr2 = matcher.refine(sad, pl2, pr2);
Point2i pl3(362,32); Point2i pr3(306,32); pr3 = matcher.refine(sad, pl3, pr3);
Point2i pl4(213,155); Point2i pr4(155,159); pr4 = matcher.refine(sad, pl4, pr4);
Point2i pl5(96,209); Point2i pr5(36,210); pr5 = matcher.refine(sad, pl5, pr5);
Point2i pl6(330,212); Point2i pr6(269,211); pr6 = matcher.refine(sad, pl6, pr6);
Point2i pl7(276,241); Point2i pr7(216,242); pr7 = matcher.refine(sad, pl7, pr7);
Point2i pl8(385,332); Point2i pr8(321,328); pr8 = matcher.refine(sad, pl8, pr8);
Point2i pl9(180,389); Point2i pr9(114,388); pr9 = matcher.refine(sad, pl9, pr9);
Point2i pl10(136,500); Point2i pr10(67,500); pr10 = matcher.refine(sad, pl10, pr10);
fm.addCorrespondence(pl1, pr1);
fm.addCorrespondence(pl2, pr2);
fm.addCorrespondence(pl3, pr3);
fm.addCorrespondence(pl4, pr4);
fm.addCorrespondence(pl5, pr5);
fm.addCorrespondence(pl6, pr6);
fm.addCorrespondence(pl7, pr7);
fm.addCorrespondence(pl8, pr8);
// fm.addCorrespondence(pl9, pr9);
// fm.addCorrespondence(pl10, pr10);
fm.estimate();
}
UINT16 wangpc_emb_device::wangpcbus_iorc_r(address_space &space, offs_t offset, UINT16 mem_mask)
{
UINT16 data = 0xffff;
if (sad(offset))
{
switch (offset & 0x7f)
{
case 0xc0/2:
data = m_option;
break;
case 0xfe/2:
data = 0xfc00 | (m_parity_odd << 9) | (m_parity_error << 8) | OPTION_ID;
break;
}
logerror("emb read %06x:%02x\n", offset*2, data);
}
return data;
}
void wangpc_mvc_device::wangpcbus_aiowc_w(address_space &space, offs_t offset, UINT16 mem_mask, UINT16 data)
{
if (sad(offset) && ACCESSING_BITS_0_7)
{
switch (offset & 0x7f)
{
case 0x00/2:
m_crtc->address_w(space, 0, data & 0xff);
break;
case 0x02/2:
m_crtc->register_w(space, 0, data & 0xff);
break;
case 0x10/2:
case 0x12/2:
if (LOG) logerror("MVC option %02x\n", data & 0xff);
m_option = data & 0xff;
break;
}
}
}
inline int sum(const simd_slice_t<xmm_u8,8,16,1> &x)
{
const xmm_u16 &t = sad(x.array, xmm_u8::zero());
return extract<4>(t);
}
void CompleteLinkage::operator()(DistanceMatrix<float> & original_distance, std::vector<BinaryTreeNode> & cluster_tree, const float threshold /*=1*/) const
{
// attention: clustering process is done by clustering the indices
// pointing to elements in inputvector and distances in inputmatrix
// input MUST have >= 2 elements!
if (original_distance.dimensionsize() < 2)
{
throw ClusterFunctor::InsufficientInput(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "Distance matrix to start from only contains one element");
}
std::vector<std::set<Size> > clusters(original_distance.dimensionsize());
for (Size i = 0; i < original_distance.dimensionsize(); ++i)
{
clusters[i].insert(i);
}
cluster_tree.clear();
cluster_tree.reserve(original_distance.dimensionsize() - 1);
// Initial minimum-distance pair
original_distance.updateMinElement();
std::pair<Size, Size> min = original_distance.getMinElementCoordinates();
Size overall_cluster_steps(original_distance.dimensionsize());
startProgress(0, original_distance.dimensionsize(), "clustering data");
while (original_distance(min.first, min.second) < threshold)
{
//grow the tree
cluster_tree.push_back(BinaryTreeNode(*(clusters[min.second].begin()), *(clusters[min.first].begin()), original_distance(min.first, min.second)));
if (cluster_tree.back().left_child > cluster_tree.back().right_child)
{
std::swap(cluster_tree.back().left_child, cluster_tree.back().right_child);
}
if (original_distance.dimensionsize() > 2)
{
//pick minimum-distance pair i,j and merge them
//pushback elements of second to first (and then erase second)
clusters[min.second].insert(clusters[min.first].begin(), clusters[min.first].end());
// erase first one
clusters.erase(clusters.begin() + min.first);
//update original_distance matrix
//complete linkage: new distance between clusters is the minimum distance between elements of each cluster
//lance-williams update for d((i,j),k): 0.5* d(i,k) + 0.5* d(j,k) + 0.5* |d(i,k)-d(j,k)|
for (Size k = 0; k < min.second; ++k)
{
float dik = original_distance.getValue(min.first, k);
float djk = original_distance.getValue(min.second, k);
original_distance.setValueQuick(min.second, k, (0.5f * dik + 0.5f * djk + 0.5f * std::fabs(dik - djk)));
}
for (Size k = min.second + 1; k < original_distance.dimensionsize(); ++k)
{
float dik = original_distance.getValue(min.first, k);
float djk = original_distance.getValue(min.second, k);
original_distance.setValueQuick(k, min.second, (0.5f * dik + 0.5f * djk + 0.5f * std::fabs(dik - djk)));
}
//reduce
original_distance.reduce(min.first);
//update minimum-distance pair
original_distance.updateMinElement();
//get new min-pair
min = original_distance.getMinElementCoordinates();
}
else
{
break;
}
setProgress(overall_cluster_steps - original_distance.dimensionsize());
//repeat until only two cluster remains or threshold exceeded, last step skips matrix operations
}
//fill tree with dummy nodes
Size sad(*clusters.front().begin());
for (Size i = 1; i < clusters.size() && (cluster_tree.size() < cluster_tree.capacity()); ++i)
{
cluster_tree.push_back(BinaryTreeNode(sad, *clusters[i].begin(), -1.0));
}
//~ while(cluster_tree.size() < cluster_tree.capacity())
//~ {
//~ cluster_tree.push_back(BinaryTreeNode(0,1,-1.0));
//~ }
endProgress();
}
void AverageLinkage::operator()(DistanceMatrix<float> & original_distance, std::vector<BinaryTreeNode> & cluster_tree, const float threshold /*=1*/) const
{
// input MUST have >= 2 elements!
if (original_distance.dimensionsize() < 2)
{
throw ClusterFunctor::InsufficientInput(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "Distance matrix to start from only contains one element");
}
std::vector<std::set<Size> > clusters(original_distance.dimensionsize());
for (Size i = 0; i < original_distance.dimensionsize(); ++i)
{
clusters[i].insert(i);
}
cluster_tree.clear();
cluster_tree.reserve(original_distance.dimensionsize() - 1);
// Initial minimum-distance pair
original_distance.updateMinElement();
std::pair<Size, Size> min = original_distance.getMinElementCoordinates();
Size overall_cluster_steps(original_distance.dimensionsize());
startProgress(0, original_distance.dimensionsize(), "clustering data");
while (original_distance(min.second, min.first) < threshold)
{
//grow the tree
cluster_tree.push_back(BinaryTreeNode(*(clusters[min.second].begin()), *(clusters[min.first].begin()), original_distance(min.first, min.second)));
if (cluster_tree.back().left_child > cluster_tree.back().right_child)
{
std::swap(cluster_tree.back().left_child, cluster_tree.back().right_child);
}
if (original_distance.dimensionsize() > 2)
{
//pick minimum-distance pair i,j and merge them
//calculate parameter for lance-williams formula
float alpha_i = (float)(clusters[min.first].size() / (float)(clusters[min.first].size() + clusters[min.second].size()));
float alpha_j = (float)(clusters[min.second].size() / (float)(clusters[min.first].size() + clusters[min.second].size()));
//~ std::cout << alpha_i << '\t' << alpha_j << std::endl;
//pushback elements of second to first (and then erase second)
clusters[min.second].insert(clusters[min.first].begin(), clusters[min.first].end());
// erase first one
clusters.erase(clusters.begin() + min.first);
//update original_distance matrix
//average linkage: new distance between clusters is the minimum distance between elements of each cluster
//lance-williams update for d((i,j),k): (m_i/m_i+m_j)* d(i,k) + (m_j/m_i+m_j)* d(j,k) ; m_x is the number of elements in cluster x
for (Size k = 0; k < min.second; ++k)
{
float dik = original_distance.getValue(min.first, k);
float djk = original_distance.getValue(min.second, k);
original_distance.setValueQuick(min.second, k, (alpha_i * dik + alpha_j * djk));
}
for (Size k = min.second + 1; k < original_distance.dimensionsize(); ++k)
{
float dik = original_distance.getValue(min.first, k);
float djk = original_distance.getValue(min.second, k);
original_distance.setValueQuick(k, min.second, (alpha_i * dik + alpha_j * djk));
}
//reduce
original_distance.reduce(min.first);
//update minimum-distance pair
original_distance.updateMinElement();
//get min-pair from triangular matrix
min = original_distance.getMinElementCoordinates();
}
else
{
break;
}
setProgress(overall_cluster_steps - original_distance.dimensionsize());
//repeat until only two cluster remains, last step skips matrix operations
}
//fill tree with dummy nodes
Size sad(*clusters.front().begin());
for (Size i = 1; (i < clusters.size()) && (cluster_tree.size() < cluster_tree.capacity()); ++i)
{
cluster_tree.push_back(BinaryTreeNode(sad, *clusters[i].begin(), -1.0));
}
endProgress();
}
void
FeatureMatcher::matchFeatures(PyramidLevel* ref_level,
PyramidLevel* target_level,
const std::vector<std::vector<int> >& candidates,
FeatureMatch* matches,
int* num_matches)
{
int num_ref_features = ref_level->getNumKeypoints();
int num_target_features = target_level->getNumKeypoints();
// allocate more space for buffers if necessary
if (num_ref_features > _ref_feature_capacity) {
_ref_to_target_indices = (int32_t*) realloc(_ref_to_target_indices, num_ref_features * sizeof(int32_t));
_ref_to_target_scores = (int32_t*) realloc(_ref_to_target_scores, num_ref_features * sizeof(int32_t));
_ref_feature_capacity = num_ref_features;
}
if (num_target_features > _target_feature_capacity) {
int buf_size = num_target_features * sizeof(int32_t);
_target_to_ref_indices = (int32_t*) realloc(_target_to_ref_indices, buf_size);
_target_to_ref_scores = (int32_t*) realloc(_target_to_ref_scores, buf_size);
_target_feature_capacity = num_target_features;
}
int descriptor_len = ref_level->getDescriptorLength();
assert(descriptor_len == target_level->getDescriptorLength());
SAD sad(descriptor_len);
int32_t worst_score = sad.getWorstScore();
// initialize book-keeping for feature matching
for (int i = 0; i < num_ref_features; i++) {
_ref_to_target_scores[i] = worst_score + 1;
_ref_to_target_indices[i] = -1;
}
for (int i = 0; i < num_target_features; i++) {
_target_to_ref_scores[i] = worst_score + 1;
_target_to_ref_indices[i] = -1;
}
// for each feature in the target frame, compute the best matching feature in
// the reference frame.
// Similarly, compute the best matching feature in the target frame for each
// feature in the reference frame.
// Match score is defined as the sum of absolute differences between two feature
// descriptors. Lower scores (less difference) are better.
for (int ref_ind = 0; ref_ind < num_ref_features; ref_ind++) {
const uint8_t * ref_desc = ref_level->getDescriptor(ref_ind);
const std::vector<int>& ref_candidates(candidates[ref_ind]);
for (std::vector<int>::const_iterator ref_candidates_itr = ref_candidates.begin(),
ref_candidates_end = ref_candidates.end();
ref_candidates_itr != ref_candidates_end;
++ref_candidates_itr) {
int target_ind = *ref_candidates_itr;
const uint8_t * target_desc = target_level->getDescriptor(target_ind);
int score = sad.score(ref_desc, target_desc);
assert(score <= worst_score);
// see if this score is the best for either descriptor
if (score < _ref_to_target_scores[ref_ind]) {
_ref_to_target_scores[ref_ind] = score;
_ref_to_target_indices[ref_ind] = target_ind;
}
if (score < _target_to_ref_scores[target_ind]) {
_target_to_ref_scores[target_ind] = score;
_target_to_ref_indices[target_ind] = ref_ind;
}
}
}
// now find features that are mutual best matches in both directions
for (int ref_ind = 0; ref_ind < num_ref_features; ref_ind++) {
int target_ind = _ref_to_target_indices[ref_ind];
if (target_ind >= 0 &&
_target_to_ref_indices[target_ind] == ref_ind) {
KeypointData* ref_kpdata = ref_level->getKeypointData(ref_ind);
KeypointData* target_kpdata = target_level->getKeypointData(target_ind);
assert(ref_kpdata->kp.u >= 0 && ref_kpdata->kp.v >= 0 &&
ref_kpdata->kp.u < ref_level->getWidth() &&
ref_kpdata->kp.v < ref_level->getHeight());
assert(target_kpdata->kp.u >= 0 && target_kpdata->kp.v >= 0 &&
target_kpdata->kp.u < target_level->getWidth() &&
target_kpdata->kp.v < target_level->getHeight());
FeatureMatch match(target_kpdata, ref_kpdata);
match.status = MATCH_OK;
matches[*num_matches] = match;
(*num_matches)++;
}
}
}
void perform() {
ImageChannel imgDepth(desc.imgLeft.getWidth(), desc.imgLeft.getHeight());
imgDepth.ones();
CV::Gauss g(1.0);
desc.imgLeft = g.filter(desc.imgLeft);
desc.imgRight = g.filter(desc.imgRight);
MatchingSAD sad(desc.imgLeft, desc.imgRight, 13);
MatchingConvolution conv(desc.imgLeft, desc.imgRight, 9);
Matching matcher;
DataMatrix<SimpleFeatures::FeatureVec> fLeft = SimpleFeatures::getFeatures(desc.imgLeft, 25);
DataMatrix<SimpleFeatures::FeatureVec> fRight = SimpleFeatures::getFeatures(desc.imgRight, 25);
Gnuplot gp;
GnuplotPlot p;
GnuplotPlotElementLines errLine;
p.add(&errLine);
GnuplotSplot splot;
GnuplotSplotElementPoints depthPlot;
splot.add(&depthPlot);
DataMatrix<std::vector<float>> hogLeft(desc.imgLeft.getWidth(), desc.imgRight.getHeight());
DataMatrix<std::vector<float>> hogRight(desc.imgLeft.getWidth(), desc.imgRight.getHeight());
HOG hog1(desc.imgLeft, 10);
HOG hog2(desc.imgRight, 10);
// for (int y = 0; y < desc.imgLeft.getHeight(); ++y) {
// for (int x = 0; x < desc.imgLeft.getWidth(); ++x) {
// std::vector<HOGGradient> gLeft = hog1.getGradients(desc.imgLeft, x, y);
// std::vector<HOGGradient> gRight = hog2.getGradients(desc.imgRight, x, y);
// hog.relativeToBestOne(gLeft);
// hog.relativeToBestOne(gRight);
// hog.normalize(gLeft);
// hog.normalize(gRight);
// hogLeft.set(x,y,hog.binify(gLeft, 8));
// hogRight.set(x,y,hog.binify(gRight, 8));
// }
// }
struct QueueElem {
float err;
Point2i p;
QueueElem(const float err, const Point2i p) : err(err), p(p) {;}
QueueElem() : err(99999999), p(0,0) {;}
bool operator < (const QueueElem& o) const {return err > o.err;}
};
// auto comp = [] (const QueueElem& a, const QueueElem& b) {
// return a.err < b.err;
// };
//#pragma omp parallel 0
for (int y = 0; y < desc.imgLeft.getHeight()-1; y+=2) {
std::cout << y << std::endl;
for (int x = 0; x < desc.imgLeft.getWidth()-1; x+=2) {
const Point2i pL(x,y);
// std::cout << "sigma: " << fLeft.get(pL.x, pL.y).sigma << std::endl;
// std::cout << "asigma: " << fLeft.get(pL.x, pL.y).avgSigma << std::endl;
// std::cout << "agradx: " << fLeft.get(pL.x, pL.y).avgGradX << std::endl;
// std::cout << "agrady: " << fLeft.get(pL.x, pL.y).avgGradY << std::endl;
const Eigen::Vector3d l = desc.fm.getEpilineRight(pL);
const int x1 = 0;
const int x2 = desc.imgLeft.getWidth();
const int y1 = -(l(0)*x1 + l(2)) / l(1);
const int y2 = -(l(0)*x2 + l(2)) / l(1);
BresenhamIter iter(x1,y1,x2,y2);
std::priority_queue<QueueElem, std::vector<QueueElem>> queue;
// follow the epipolar line
while (iter.hasNext()) {
const Point2i _pR = iter.next();
const int maxD = desc.imgLeft.getWidth() / 4;
if(pL.getDistance(_pR) > maxD) {continue;}
//if(pL.getDistance(pR) < 10) {continue;}
//const int w = 0;
for (int oy = -1; oy <= +1; ++oy) {
// for (int ox = -w; ox <= +w; ++ox) {
const Point2i pR = _pR + (Point2i(0, oy));
if (pR.x < 2 || pR.y < 2 || pR.x >= desc.imgRight.getWidth() - 2 || pR.y >= desc.imgRight.getHeight() - 2) {continue;}
//float err = - fLeft.get(pL.x, pL.y).diff(fRight.get(pR.x, pR.y));
float err = sad.getError(pL, pR);
//float err = -conv.getScore(pL, pR);
//float err = HOG::getDistance( hogLeft.get(pL.x, pL.y), hogRight.get(pR.x, pR.y) );
//if (err < vMin) {vMin = err; pMin = pR;}
queue.push(QueueElem(err, pR));
//errLine.add( GnuplotPoint2(pR.x, err) );
//std::cout << pR.x << "," << pR.y << std::endl;
// }
}
}
// gp.draw(p);
// gp.flush();
// usleep(1000*1000);
if (queue.size() > 10) {
Point2i best = queue.top().p;
int cnt = 8;
Point2i sum(0,0);
for (int i = 0; i < cnt; ++i) {
sum += queue.top().p;
queue.pop();
}
sum /= cnt;
//if (best.getDistance(sum) < 25) {
const float depth = pL.getDistance(best);
imgDepth.set(pL.x, pL.y, depth);
imgDepth.set(pL.x+1, pL.y, depth);
imgDepth.set(pL.x, pL.y+1, depth);
imgDepth.set(pL.x+1, pL.y+1, depth);
depthPlot.add(GnuplotPoint3(best.x, best.y, depth));
//}
}
// if (vMin != INFINITY ) {
// const float depth = pL.getDistance(bestInRight.p);
// imgDepth.set(pL.x, pL.y, depth);
// imgDepth.set(pL.x+1, pL.y, depth);
// imgDepth.set(pL.x, pL.y+1, depth);
// imgDepth.set(pL.x+1, pL.y+1, depth);
// } else {
// int i = 0; (void) i;
// }
}
}
gp.draw(splot);
gp.flush();
CV::Normalize::inplace(imgDepth);
ImageFactory::writePNG("/tmp/depth.png", imgDepth);
}
