int OnKeyValue(KeyObject* k, ValueObject* v, uint32 cursor)
{
ZSetKeyObject* zsk = (ZSetKeyObject*) k;
bool need_delete = false;
need_delete =
z_containmin ?
zsk->score >= z_min_score :
zsk->score > z_min_score;
if (need_delete)
{
need_delete =
z_containmax ?
zsk->score <= z_max_score :
zsk->score < z_max_score;
}
if (need_delete)
{
ZSetScoreKeyObject zk(zsk->key, zsk->value, zsk->db);
z_db->DelValue(zk);
z_db->DelValue(*zsk);
z_meta.size--;
z_count++;
}
if (zsk->score == z_max_score)
{
return -1;
}
return 0;
}
int main(int argc, char ** argv)
{
try
{
if (argc != 2)
{
std::cerr << "usage: " << argv[0] << " hosts" << std::endl;
return 2;
}
Poco::AutoPtr<Poco::ConsoleChannel> channel = new Poco::ConsoleChannel(std::cerr);
Logger::root().setChannel(channel);
Logger::root().setLevel("trace");
zkutil::ZooKeeper zk(argv[1]);
std::string unused;
zk.tryCreate("/test", "", zkutil::CreateMode::Persistent, unused);
std::cerr << "Please run `./nozk.sh && sleep 40s && ./yeszk.sh`" << std::endl;
time_t time0 = time(0);
while (true)
{
{
zkutil::Ops ops;
ops.emplace_back(std::make_unique<zkutil::Op::Create>("/test/zk_expiration_test", "hello", zk.getDefaultACL(), zkutil::CreateMode::Persistent));
ops.emplace_back(std::make_unique<zkutil::Op::Remove>("/test/zk_expiration_test", -1));
int code;
try
{
code = zk.tryMulti(ops);std::string unused;
//code = zk.tryCreate("/test", "", zkutil::CreateMode::Persistent, unused);
}
catch (zkutil::KeeperException & e)
{
code = e.code;
}
std::cout << time(0) - time0 << "s: " << zkutil::ZooKeeper::error2string(code) << std::endl;
}
sleep(1);
}
}
catch (zkutil::KeeperException & e)
{
std::cerr << "KeeperException: " << e.displayText() << std::endl;
return 1;
}
catch (...)
{
std::cerr << "Some exception" << std::endl;
return 2;
}
return 0;
}
int Ardb::ZScore(const DBID& db, const Slice& key, const Slice& value,
double& score)
{
ZSetScoreKeyObject zk(key, value, db);
ValueObject zv;
if (0 != GetValue(zk, &zv))
{
return ERR_NOT_EXIST;
}
score = zv.v.double_v;
return 0;
}
int main(int argc, char ** argv)
{
try
{
if (argc != 3)
{
std::cerr << "usage: " << argv[0] << " <zookeeper_config> <number_of_watches>" << std::endl;
return 3;
}
ConfigProcessor processor(false, true);
auto config = processor.loadConfig(argv[1]).configuration;
zkutil::ZooKeeper zk(*config, "zookeeper");
zkutil::EventPtr watch = std::make_shared<Poco::Event>();
/// NOTE: setting watches in multiple threads because doing it in a single thread is too slow.
size_t watches_per_thread = std::stoull(argv[2]) / N_THREADS;
std::vector<std::thread> threads;
for (size_t i_thread = 0; i_thread < N_THREADS; ++i_thread)
{
threads.emplace_back([&, i_thread]
{
for (size_t i = 0; i < watches_per_thread; ++i)
zk.exists("/clickhouse/nonexistent_node" + std::to_string(i * N_THREADS + i_thread), nullptr, watch);
});
}
for (size_t i_thread = 0; i_thread < N_THREADS; ++i_thread)
threads[i_thread].join();
while (true)
{
std::cerr << "WAITING..." << std::endl;
sleep(10);
}
}
catch (Poco::Exception & e)
{
std::cerr << "Exception: " << e.displayText() << std::endl;
return 1;
}
catch (std::exception & e)
{
std::cerr << "std::exception: " << e.what() << std::endl;
return 3;
}
catch (...)
{
std::cerr << "Some exception" << std::endl;
return 2;
}
return 0;
}
TEST(push_communication, update_members) {
jubatus::util::lang::shared_ptr<common::lock_service> zk(new zk_stub());
const pair<string, int> my_id(std::make_pair("127.0.0.1", 1112));
jubatus::util::lang::shared_ptr<push_communication> com =
push_communication::create(zk, "test_type", "test_name", 1, my_id);
ASSERT_EQ(3, com->update_members()); // must not be 4
vector<pair<string, int> > list = com->servers_list();
ASSERT_EQ(3, list.size());
for (size_t i = 0; i < list.size(); ++i) {
ASSERT_NE(list[i], my_id); // must not include "127.0.0.1:1112"
}
}
/*
* returns -1:no change 0:no meta&zset size change
* 1:meta change & no zset size change
* 2:meta change & zset size change
*/
int Ardb::TryZAdd(const DBID& db, const Slice& key, ZSetMetaValue& meta,
double score, const Slice& value)
{
bool metachange = false;
if (score > meta.max_score)
{
meta.max_score = score;
metachange = true;
}
if (score < meta.min_score)
{
meta.min_score = score;
metachange = true;
}
ZSetScoreKeyObject zk(key, value, db);
ValueObject zv;
if (0 != GetValue(zk, &zv))
{
meta.size++;
zv.type = DOUBLE;
zv.v.double_v = score;
SetValue(zk, zv);
ZSetKeyObject zsk(key, value, score, db);
ValueObject zsv;
zsv.type = EMPTY;
SetValue(zsk, zsv);
return 2;
} else
{
if (zv.v.double_v != score)
{
ZSetKeyObject zsk(key, value, zv.v.double_v, db);
DelValue(zsk);
zsk.score = score;
ValueObject zsv;
zsv.type = EMPTY;
SetValue(zsk, zsv);
zv.type = DOUBLE;
zv.v.double_v = score;
SetValue(zk, zv);
return metachange ? 1 : 0;
}
}
return -1;
}
int OnKeyValue(KeyObject* k, ValueObject* v, uint32 cursor)
{
ZSetKeyObject* zsk = (ZSetKeyObject*) k;
if (rank >= z_start && rank <= z_stop)
{
ZSetScoreKeyObject zk(zsk->key, zsk->value, zsk->db);
z_db->DelValue(zk);
z_db->DelValue(*zsk);
z_meta.size--;
z_count++;
}
rank++;
if (rank > z_stop)
{
return -1;
}
return 0;
}
int Ardb::Type(const DBID& db, const Slice& key)
{
if (GetValue(db, key, NULL) == 0)
{
return KV;
}
int type = -1;
Slice empty;
KeyObject sk(key, SET_META, db);
if (0 == GetValue(sk, NULL))
{
return SET_ELEMENT;
}
KeyObject lk(key, LIST_META, db);
if (0 == GetValue(lk, NULL))
{
return LIST_META;
}
KeyObject zk(key, ZSET_META, db);
if (0 == GetValue(zk, NULL))
{
return ZSET_ELEMENT_SCORE;
}
HashKeyObject hk(key, empty, db);
GET_KEY_TYPE(hk, type);
if (type > 0)
{
return type;
}
KeyObject tk(key, TABLE_META, db);
if (0 == GetValue(tk, NULL))
{
return TABLE_META;
}
KeyObject bk(key, BITSET_META, db);
if (0 == GetValue(bk, NULL))
{
return BITSET_META;
}
return -1;
}
int Ardb::ZRem(const DBID& db, const Slice& key, const Slice& value)
{
KeyLockerGuard keyguard(m_key_locker, db, key);
ZSetScoreKeyObject zk(key, value, db);
ValueObject zv;
if (0 == GetValue(zk, &zv))
{
BatchWriteGuard guard(GetEngine());
DelValue(zk);
ZSetKeyObject zsk(key, value, zv.v.double_v, db);
DelValue(zsk);
ZSetMetaValue meta;
if (0 == GetZSetMetaValue(db, key, meta))
{
meta.size--;
SetZSetMetaValue(db, key, meta);
}
return 0;
}
return ERR_NOT_EXIST;
}
int Ardb::ZIncrby(const DBID& db, const Slice& key, double increment,
const Slice& value, double& score)
{
KeyLockerGuard keyguard(m_key_locker, db, key);
ZSetScoreKeyObject zk(key, value, db);
ValueObject zv;
if (0 == GetValue(zk, &zv))
{
BatchWriteGuard guard(GetEngine());
SetValue(zk, zv);
ZSetKeyObject zsk(key, value, zv.v.double_v, db);
DelValue(zsk);
zsk.score += increment;
ValueObject zsv;
zsv.type = EMPTY;
SetValue(zsk, zsv);
score = zsk.score;
return 0;
}
return ERR_NOT_EXIST;
}
int Ardb::Type(const DBID& db, const Slice& key)
{
if (Exists(db, key))
{
return KV;
}
int type = -1;
Slice empty;
SetKeyObject sk(key, empty, db);
GET_KEY_TYPE(sk, type);
if (type < 0)
{
ZSetScoreKeyObject zk(key, empty, db);
GET_KEY_TYPE( zk, type);
if (type < 0)
{
HashKeyObject hk(key, empty, db);
GET_KEY_TYPE( hk, type);
if (type < 0)
{
KeyObject lk(key, LIST_META, db);
GET_KEY_TYPE( lk, type);
if (type < 0)
{
KeyObject tk(key, TABLE_META, db);
GET_KEY_TYPE(tk, type);
if (type < 0)
{
KeyObject bk(key, BITSET_META, db);
GET_KEY_TYPE(bk, type);
}
}
}
}
}
return type;
}
//0:success
//1:参数错误
//2:执行结果错误
int main(int argc ,char** argv)
{
FILE * outFd = NULL;
int iRet = parseArg(argc, argv);
if (0 == iRet) return 0;
if (-1 == iRet) return 1;
if (g_strOut.length())
{
outFd = fopen(g_strOut.c_str(), "w+b");
if (!outFd){
printf("Failure to open output file:%s, errno=%d\n", g_strOut.c_str(), errno);
return 1;
}
}
ZkToolAdaptor zk(g_strHost);
if (0 != zk.init()){
output(outFd, 2, zk.getErrCode(), "msg: Failure to init zk, err=%s\n", zk.getErrMsg());
if (outFd) fclose(outFd);
return 2;
}
if (0 != zk.connect())
{
output(outFd, 2, zk.getErrCode(), "msg: Failure to connect zk, err=%s\n", zk.getErrMsg());
if (outFd) fclose(outFd);
return 2;
}
int timeout = 5000;
while(timeout > 0){
if (!zk.isConnected()){
timeout --;
ZkAdaptor::sleep(1);
continue;
}
//add auth
if (g_auth.size())
{
list<string>::iterator iter = g_auth.begin();
while(iter != g_auth.end())
{
if (!zk.addAuth("digest", iter->c_str(), iter->length(), 3000))
{
output(outFd, 2, 0,"msg: Failure to auth, err=%s\n", zk.getErrMsg());
if (outFd) fclose(outFd);
return 2;
}
iter++;
}
}
struct ACL_vector acl;
struct ACL_vector *pacl=&ZOO_OPEN_ACL_UNSAFE;
if (g_priv.size())
{
acl.count = g_priv.size();
acl.data = new struct ACL[acl.count];
int index=0;
list<string>::iterator iter = g_priv.begin();
while(iter != g_priv.end())
{
if (!ZkAdaptor::fillAcl(iter->c_str(), acl.data[index++]))
{
output(outFd, 2, 0,"msg: invalid auth %s\n", iter->c_str());
if (outFd) fclose(outFd);
return 2;
}
iter++;
}
pacl = &acl;
}
int flags = 0;
if (g_sequence) flags |= ZOO_SEQUENCE;
if (g_ephemeral) flags |= ZOO_EPHEMERAL;
char path[2048];
int ret = zk.createNode(g_strNode, g_strValue.c_str(), g_strValue.length(), pacl, flags, g_recursive, path, 2048);
if (-1 == ret){
output(outFd, 2, zk.getErrCode(), "msg: Failure to create node, err=%s\n", zk.getErrMsg());
if (outFd) fclose(outFd);
return 2;
}
if (0 == ret){
output(outFd, 2, zk.getErrCode(), NULL, "msg: node exists\n");
if (outFd) fclose(outFd);
return 2;
}
output(outFd, 0, 0, "node: %s\nmsg: success\n", !g_sequence?g_strNode.c_str():path);
if (outFd) fclose(outFd);
return 0;
}
output(outFd, 2, 0, NULL, "msg: Timeout to connect zk\n");
if (outFd) fclose(outFd);
return 2;
}
int Ardb::ZUnionStore(const DBID& db, const Slice& dst, SliceArray& keys,
WeightArray& weights, AggregateType type)
{
while (weights.size() < keys.size())
{
weights.push_back(1);
}
ValueScoreMap vm;
struct ZUnionWalk: public WalkHandler
{
uint32_t z_weight;
ValueScoreMap& z_vm;
AggregateType z_aggre_type;
ZUnionWalk(uint32_t ws, ValueScoreMap& vm, AggregateType type) :
z_weight(ws), z_vm(vm), z_aggre_type(type)
{
}
int OnKeyValue(KeyObject* k, ValueObject* value, uint32 cursor)
{
ZSetKeyObject* zsk = (ZSetKeyObject*) k;
double score = 0;
switch (z_aggre_type)
{
case AGGREGATE_MIN:
{
score = z_weight * zsk->score;
if (score < z_vm[zsk->value])
{
z_vm[zsk->value] = score;
}
break;
}
case AGGREGATE_MAX:
{
score = z_weight * (zsk->score);
if (score > z_vm[zsk->value])
{
z_vm[zsk->value] = score;
}
break;
}
case AGGREGATE_SUM:
default:
{
score = z_weight * (zsk->score) + z_vm[zsk->value];
z_vm[zsk->value] = score;
break;
}
}
return 0;
}
};
SliceArray::iterator kit = keys.begin();
uint32_t idx = 0;
while (kit != keys.end())
{
ZSetMetaValue meta;
if (0 == GetZSetMetaValue(db, *kit, meta))
{
Slice empty;
ZSetKeyObject tmp(*kit, empty, meta.min_score, db);
ZUnionWalk walk(weights[idx], vm, type);
Walk(tmp, false, &walk);
}
idx++;
kit++;
}
if (vm.size() > 0)
{
double min_score = 0, max_score = 0;
BatchWriteGuard guard(GetEngine());
ZClear(db, dst);
KeyLockerGuard keyguard(m_key_locker, db, dst);
ZSetMetaValue meta;
meta.size = vm.size();
while (!vm.empty())
{
ValueScoreMap::iterator it = vm.begin();
ZSetKeyObject zsk(dst, it->first, it->second, db);
ValueObject zsv;
zsv.type = EMPTY;
ZSetScoreKeyObject zk(dst, it->first, db);
ValueObject zv;
zv.type = DOUBLE;
zv.v.double_v = it->second;
if (zv.v.double_v < min_score)
{
min_score = zv.v.double_v;
}
if (zv.v.double_v > max_score)
{
max_score = zv.v.double_v;
}
SetValue(zsk, zsv);
SetValue(zk, zv);
//reduce memory footprint for huge data set
vm.erase(it);
}
meta.min_score = min_score;
meta.max_score = max_score;
SetZSetMetaValue(db, dst, meta);
}
return vm.size();
}
//0:success
//1:参数错误
//2:执行结果错误
int main(int argc ,char** argv)
{
FILE * outFd = NULL;
g_recursive = false;
int iRet = parseArg(argc, argv);
if (0 == iRet) return 0;
if (-1 == iRet) return 1;
if (g_strOut.length())
{
outFd = fopen(g_strOut.c_str(), "w+b");
if (!outFd){
printf("Failure to open output file:%s, errno=%d\n", g_strOut.c_str(), errno);
return 1;
}
}
ZkToolAdaptor zk(g_strHost);
if (0 != zk.init()){
output(outFd, 2, zk.getErrCode(), "msg: Failure to init zk, err=%s\n", zk.getErrMsg());
if (outFd) fclose(outFd);
return 2;
}
if (0 != zk.connect())
{
output(outFd, 2, zk.getErrCode(), "msg: Failure to connect zk, err=%s\n", zk.getErrMsg());
if (outFd) fclose(outFd);
return 2;
}
int timeout = 5000;
while(timeout > 0){
if (!zk.isConnected())
{
timeout --;
ZkAdaptor::sleep(1);
continue;
}
//add auth
if (g_auth.size())
{
list<string>::iterator iter = g_auth.begin();
while(iter != g_auth.end())
{
if (!zk.addAuth("digest", iter->c_str(), iter->length(), 3000))
{
output(outFd, 2, 0, "msg: Failure to auth, err=%s\n", zk.getErrMsg());
if (outFd) fclose(outFd);
return 2;
}
iter++;
}
}
int ret = zk.deleteNode(g_strNode, g_recursive, g_version);
if (-1 == ret)
{
output(outFd, 2, zk.getErrCode(), "msg: Failure to delete node, err=%s\n", zk.getErrMsg());
if (outFd) fclose(outFd);
return 2;
}
if (0 == ret)
{
output(outFd, 2, zk.getErrCode(), NULL, "msg: node doesn't exist\n");
if (outFd) fclose(outFd);
return 2;
}
output(outFd, 0, 0, "node: %s\nmsg: success\n", g_strNode.c_str());
if (outFd) fclose(outFd);
return 0;
}
output(outFd, 2, 0, NULL, "msg: Timeout to connect zk\n");
if (outFd) fclose(outFd);
return 2;
}
int Ardb::ZInterStore(const DBID& db, const Slice& dst, SliceArray& keys,
WeightArray& weights, AggregateType type)
{
while (weights.size() < keys.size())
{
weights.push_back(1);
}
uint32_t min_size = 0;
ZSetMetaValueArray metas;
uint32_t min_idx = 0;
uint32_t idx = 0;
SliceArray::iterator kit = keys.begin();
while (kit != keys.end())
{
ZSetMetaValue meta;
if (0 == GetZSetMetaValue(db, *kit, meta))
{
if (min_size == 0 || min_size > meta.size)
{
min_size = meta.size;
min_idx = idx;
}
}
metas.push_back(meta);
kit++;
idx++;
}
struct ZInterWalk: public WalkHandler
{
uint32_t z_weight;
ValueScoreMap& z_cmp;
ValueScoreMap& z_result;
AggregateType z_aggre_type;
ZInterWalk(uint32_t weight, ValueScoreMap& cmp,
ValueScoreMap& result, AggregateType type) :
z_weight(weight), z_cmp(cmp), z_result(result), z_aggre_type(
type)
{
}
int OnKeyValue(KeyObject* k, ValueObject* value, uint32 cursor)
{
ZSetKeyObject* zsk = (ZSetKeyObject*) k;
if ((&z_cmp != &z_result) && z_cmp.count(zsk->value) == 0)
{
return 0;
}
switch (z_aggre_type)
{
case AGGREGATE_MIN:
{
double score = z_weight * zsk->score;
ValueScoreMap::iterator it = z_cmp.find(zsk->value);
if (it == z_cmp.end() || score < it->second)
{
z_result[zsk->value] = score;
}
break;
}
case AGGREGATE_MAX:
{
double score = z_weight * zsk->score;
ValueScoreMap::iterator it = z_cmp.find(zsk->value);
if (it == z_cmp.end() || score > it->second)
{
z_result[zsk->value] = score;
}
break;
}
case AGGREGATE_SUM:
default:
{
if (z_cmp.count(zsk->value) == 0)
{
z_result[zsk->value] = z_weight * (zsk->score);
} else
{
z_result[zsk->value] = z_weight * (zsk->score)
+ z_cmp[zsk->value];
}
break;
}
}
return 0;
}
};
ValueScoreMap cmp1, cmp2;
Slice empty;
ZSetKeyObject cmp_start(keys[min_idx], empty, metas[min_idx].min_score,
db);
ZInterWalk walk(weights[min_idx], cmp1, cmp1, type);
Walk(cmp_start, false, &walk);
ValueScoreMap* cmp = &cmp1;
ValueScoreMap* result = &cmp2;
for (uint32_t i = 0; i < keys.size(); i++)
{
if (i != min_idx)
{
Slice empty;
ZSetKeyObject tmp(keys.at(i), empty, metas[i].min_score, db);
ZInterWalk walk(weights[i], *cmp, *result, type);
Walk(tmp, false, &walk);
cmp->clear();
ValueScoreMap* old = cmp;
cmp = result;
result = old;
}
}
if (cmp->size() > 0)
{
double min_score = 0, max_score = 0;
BatchWriteGuard guard(GetEngine());
ZClear(db, dst);
KeyLockerGuard keyguard(m_key_locker, db, dst);
ZSetMetaValue meta;
meta.size = cmp->size();
while (!cmp->empty())
{
ValueScoreMap::iterator it = cmp->begin();
ZSetKeyObject zsk(dst, it->first, it->second, db);
ValueObject zsv;
zsv.type = EMPTY;
ZSetScoreKeyObject zk(dst, it->first, db);
ValueObject zv;
zv.type = DOUBLE;
zv.v.double_v = it->second;
if (zv.v.double_v < min_score)
{
min_score = zv.v.double_v;
}
if (zv.v.double_v > max_score)
{
max_score = zv.v.double_v;
}
SetValue(zsk, zsv);
SetValue(zk, zv);
//reduce memory footprint for huge data set
cmp->erase(it);
}
meta.min_score = min_score;
meta.max_score = max_score;
SetZSetMetaValue(db, dst, meta);
}
return cmp->size();
}
//0:success
//1:²ÎÊý´íÎó
//2:Ö´Ðнá¹û´íÎó
int main(int argc ,char** argv)
{
int iRet = parseArg(argc, argv);
if (0 == iRet) return 0;
if (-1 == iRet) return 1;
ZkToolAdaptor zk(g_strHost);
if (0 != zk.init()) {
printf("msg: Failure to init zk, err=%s\n", zk.getErrMsg());
return 2;
}
if (0 != zk.connect())
{
printf("msg: Failure to connect zk, err=%s\n", zk.getErrMsg());
return 2;
}
ZkLocker locker;
int timeout = 5000;
while(timeout > 0) {
if (!zk.isConnected()) {
timeout --;
ZkAdaptor::sleep(1);
continue;
}
//add auth
if (g_auth.size())
{
list<string>::iterator iter = g_auth.begin();
while(iter != g_auth.end())
{
if (!zk.addAuth("digest", iter->c_str(), iter->length(), 3000))
{
printf("msg: Failure to auth, err=%s\n", zk.getErrMsg());
return 2;
}
iter++;
}
}
struct ACL_vector acl;
struct ACL_vector *pacl=&ZOO_OPEN_ACL_UNSAFE;
if (g_priv.size())
{
acl.count = g_priv.size();
acl.data = new struct ACL[acl.count];
int index=0;
list<string>::iterator iter = g_priv.begin();
while(iter != g_priv.end())
{
if (!ZkAdaptor::fillAcl(iter->c_str(), acl.data[index++]))
{
printf("msg: invalid auth %s\n", iter->c_str());
return 2;
}
iter++;
}
pacl = &acl;
}
if (0 != locker.init(&zk, (char*)g_strNode.c_str(), g_strPrev, get_lock, &locker, pacl))
{
printf("failure to init zk lock\n");
return 2;
}
bool bNeedLock = true;
string lockPath;
string ownPath;
printf("Starting to lock......");
iRet = locker.lock(g_bWatchMaster);
string strSelf;
string strOwner;
string strPrev;
while(1) {
if (!zk.isConnected()) {
printf("Lost connnect..........\n");
::sleep(1);
continue;
bNeedLock = true;
}
if (0 != iRet) {
printf("Failure to lock, code=%d\n", iRet);
iRet = locker.lock(g_bWatchMaster);
}
locker.getSelfNode(strSelf);
locker.getOwnerNode(strOwner);
locker.getPrevNode(strPrev);
printf("Sleep two second.........., Locked:%s, Mine:%s, Owner:%s, Prev:%s\n", locker.isLocked()?"yes":"no", strSelf.c_str(), strOwner.c_str(), strPrev.c_str());
::sleep(4);
}
}
return 2;
}
void test_EKF_noise_ptz_data()
{
vcl_string ptz_file("/Users/jimmy/Desktop/images/33_slam_data/ptz_145420_145719.txt");
vcl_vector<PTZData> ptzs;
bool isRead = readPTZCameraFile(ptz_file.c_str(), ptzs, 1);
assert(isRead);
vcl_vector<double> gdPans;
vcl_vector<double> gdTilts;
vcl_vector<double> gdZooms;
vcl_vector<double> observedPans;
vcl_vector<double> observedTilts;
vcl_vector<double> observedZooms;
const int width = 1280;
const int height = 720;
vnl_random rnd;
double delta = 2.0;
vgl_point_2d<double> pp(width/2, height/2);
for (int i = 0; i<ptzs.size(); i++) {
gdPans.push_back(ptzs[i].pan);
gdTilts.push_back(ptzs[i].tilt);
gdZooms.push_back(ptzs[i].fl);
vpgl_perspective_camera<double> gdCamera;
bool isCamera = VxlPTZCamera::PTZToCamera(ptzs[i].fl, ptzs[i].pan, ptzs[i].tilt, gdCamera);
assert(isCamera);
// project image position
vcl_vector<vgl_point_2d<double> > worldPts;
vcl_vector<vgl_point_2d<double> > dump;
vcl_vector<vgl_point_2d<double> > imagePts;
DisneyWorldBasketballCourt::projectCalibPoints(gdCamera, width, height, worldPts, dump, imagePts, 0);
assert(worldPts.size() >= 5);
// add noise to image points
for (int j = 0; j<imagePts.size(); j++) {
double x = imagePts[j].x();
double y = imagePts[j].y();
x += delta * rnd.normal();
y += delta * rnd.normal();
imagePts[j].set(x, y);
}
// recalibrate camera
vpgl_perspective_camera<double> initCamera;
vpgl_perspective_camera<double> finalCamera;
bool isInit = VpglPlus::init_calib(worldPts, imagePts, pp, initCamera);
if (!isInit) {
printf("initiate camera error\n");
continue;
}
bool isFinal = VpglPlus::optimize_perspective_camera(worldPts, imagePts, initCamera, finalCamera);
if (!isFinal) {
printf("final camera error\n");
continue;
}
//
double pan = 0;
double tilt = 0;
double zoom = 0;
bool isPTZ = VxlPTZCamera::CameraToPTZ(finalCamera, pan, tilt, zoom);
assert(isPTZ);
observedPans.push_back(pan);
observedTilts.push_back(tilt);
observedZooms.push_back(zoom);
}
assert(gdPans.size() == observedPans.size());
assert(gdTilts.size() == observedTilts.size());
assert(gdZooms.size() == observedZooms.size());
double pan0 = gdPans[0];
double tilt0 = gdTilts[0];
double zoom0 = gdZooms[0];
vnl_vector<double> x0(6, 0);
x0[0] = pan0; x0[1] = tilt0; x0[2] = zoom0;
vnl_matrix<double> P0(6, 6, 0);
P0(0, 0) = 0.1; P0(1, 1) = 0.1; P0(2, 2) = 1.0;
P0(3, 3) = 0.01; P0(4, 4) = 0.01; P0(5, 5) = 0.1;
vnl_matrix<double> R(6, 6, 0);
R(0, 0) = 0.06; R(1, 1) = 0.02; R(2, 2) = 0.6;
R(3, 3) = 0.01; R(4, 4) = 0.005; R(5, 5) = 0.1;
vnl_matrix<double> Q(6, 6, 0);
Q(0, 0) = 0.00000004; Q(1, 1) = 0.00000004; Q(2, 2) = 0.0000004;
Q(3, 3) = 0.00000001; Q(4, 4) = 0.00000001; Q(5, 5) = 0.0000001;
PTZCameraPlaning_EKF aEKF;
aEKF.init(x0, P0);
aEKF.initNoiseCovariance(Q, R);
vcl_vector<double> smoothedPans;
vcl_vector<double> smoothedTilts;
vcl_vector<double> smoothedZooms;
for (int i = 0; i<observedPans.size(); i++) {
vnl_vector<double> zk(6, 0);
zk[0] = observedPans[i];
zk[1] = observedTilts[i];
zk[2] = observedZooms[i];
if (i >= 2) {
zk[3] = smoothedPans[i-1] - smoothedPans[i-2];
zk[4] = smoothedTilts[i-1] - smoothedTilts[i-2];
zk[5] = smoothedZooms[i-1] - smoothedZooms[i-2];
}
vnl_vector<double> xk;
vnl_matrix<double> pk;
aEKF.update(zk, xk, pk);
smoothedPans.push_back(xk[0]);
smoothedTilts.push_back(xk[1]);
smoothedZooms.push_back(xk[2]);
}
// save
vnl_vector<double> gdPanVec(&gdPans[0], (int)gdPans.size());
vnl_vector<double> observedPanVec(&observedPans[0], (int)observedPans.size());
vnl_vector<double> smoothedPanVec(&smoothedPans[0], (int)smoothedPans.size());
vnl_vector<double> gdZoomVec(&gdZooms[0], (int)gdZooms.size());
vnl_vector<double> observedZoomVec(&observedZooms[0], (int)observedZooms.size());
vnl_vector<double> smoothedZoomVec(&smoothedZooms[0], (int)smoothedZooms.size());
vnl_vector<double> gdTiltVec(&gdTilts[0], (int)gdTilts.size());
vnl_vector<double> observedTiltVec(&observedTilts[0], (int)observedTilts.size());
vnl_vector<double> smoothedTiltVec(&smoothedTilts[0], (int)observedTilts.size());
vcl_string save_file("EKF_simulated_ptz.mat");
vnl_matlab_filewrite awriter(save_file.c_str());
awriter.write(gdPanVec, "gdPan");
awriter.write(observedPanVec, "observedPan");
awriter.write(smoothedPanVec, "sm_pan");
awriter.write(gdZoomVec, "gdZoom");
awriter.write(observedZoomVec, "observedZoom");
awriter.write(smoothedZoomVec, "smoothedZoom");
awriter.write(gdTiltVec, "gdTilt");
awriter.write(vnl_vector<double>(&observedTilts[0], (int)observedTilts.size()), "observedTilt");
awriter.write(vnl_vector<double>(&smoothedTilts[0], (int)observedTilts.size()), "smoothedTilt");
printf("save to %s\n", save_file.c_str());
}
void test_EKF_interface_simulated_data()
{
vcl_string panFile("/Users/jimmy/Data/pan_regression/real_data/RF_raw_output.txt");
vcl_string velocityFile("/Users/jimmy/Data/pan_regression/real_data/RF_raw_velocity.txt");
vcl_string panGdFile("/Users/jimmy/Data/pan_regression/real_data/quarter_2_spherical_map/testing_fn_pan_features.txt");
vnl_matrix<double> panMat;
vnl_matrix<double> velocityMat;
vnl_matrix<double> panGdMat;
VnlPlus::readMat(panFile.c_str(), panMat);
VnlPlus::readMat(velocityFile.c_str(), velocityMat);
VnlPlus::readMat(panGdFile.c_str(), panGdMat);
vcl_vector<double> observed_pan;
vcl_vector<double> observed_velocity;
for (int i = 0; i<panMat.rows(); i++) {
observed_pan.push_back(panMat(i, 1));
observed_velocity.push_back(velocityMat(i, 1));
}
vcl_vector<double> smoothed_pan;
vcl_vector<double> smoothed_velocity;
vnl_vector<double> x0(2, 0);
x0[0] = 12; x0[1] = 0.0;
vnl_matrix<double> P0(2, 2, 0);
P0(0, 0) = 0.1; P0(1, 1) = 0.1;
vnl_matrix<double> R(2, 2, 0);
R(0, 0) = 0.06; R(1, 1) = 0.01;
vnl_matrix<double> Q(2, 2, 0);
Q(0, 0) = 0.00000004; Q(1, 1) = 0.00000001;
cameraPlaningPan_EKF aEKF;
aEKF.init(x0, P0);
aEKF.initNoiseCovariance(Q, R);
for (int i = 0; i<observed_pan.size(); i++) {
vnl_vector<double> zk(2, 0);
zk[0] = observed_pan[i];
zk[1] = observed_velocity[i];
vnl_vector<double> xk;
vnl_matrix<double> pk;
aEKF.update(zk, xk, pk);
smoothed_pan.push_back(xk[0]);
smoothed_velocity.push_back(xk[1]);
}
vnl_vector<double> smoothedPanVec(&smoothed_pan[0], (int)smoothed_pan.size());
vnl_vector<double> smoothedVelocityVec(&smoothed_velocity[0], (int)smoothed_velocity.size());
vcl_string save_file("EKF_observed_pan_velocity_26.mat");
vnl_matlab_filewrite awriter(save_file.c_str());
awriter.write(smoothedPanVec, "sm_pan");
awriter.write(panMat.get_column(1), "pan");
awriter.write(panGdMat.get_column(1), "gdPan");
awriter.write(velocityMat.get_column(1), "velo");
awriter.write(smoothedVelocityVec, "sm_velo");
printf("save to %s\n", save_file.c_str());
}
void test_EKF_Panonly_noise_ptz_data()
{
vcl_string ptz_file("/Users/jimmy/Desktop/images/33_slam_data/ptz_145420_145719.txt");
vcl_vector<PTZData> ptzs;
bool isRead = readPTZCameraFile(ptz_file.c_str(), ptzs, 1);
assert(isRead);
vcl_vector<double> gdPans;
vcl_vector<double> gdTilts;
vcl_vector<double> gdZooms;
vcl_vector<double> observedPans;
vcl_vector<double> observedTilts;
vcl_vector<double> observedZooms;
vcl_vector<vcl_vector<vgl_point_2d<double> > > observedImagePts;
const int width = 1280;
const int height = 720;
vnl_random rnd;
double delta = 2.0;
vgl_point_2d<double> pp(width/2, height/2);
vcl_vector<vgl_point_2d<double> > firstWorldPts;
for (int i = 0; i<ptzs.size(); i++) {
gdPans.push_back(ptzs[i].pan);
gdTilts.push_back(ptzs[i].tilt);
gdZooms.push_back(ptzs[i].fl);
vpgl_perspective_camera<double> gdCamera;
bool isCamera = VxlPTZCamera::PTZToCamera(ptzs[i].fl, ptzs[i].pan, ptzs[i].tilt, gdCamera);
assert(isCamera);
// project image position
vcl_vector<vgl_point_2d<double> > worldPts;
vcl_vector<vgl_point_2d<double> > dump;
vcl_vector<vgl_point_2d<double> > imagePts;
DisneyWorldBasketballCourt::projectCalibPoints(gdCamera, width, height, worldPts, dump, imagePts, 10000);
// assert(worldPts.size() == 35);
if (firstWorldPts.size() == 0) {
firstWorldPts = worldPts;
}
assert(firstWorldPts.size() == worldPts.size());
// add noise to image points
for (int j = 0; j<imagePts.size(); j++) {
double x = imagePts[j].x();
double y = imagePts[j].y();
x += delta * rnd.normal();
y += delta * rnd.normal();
imagePts[j].set(x, y);
}
observedImagePts.push_back(imagePts);
// recalibrate camera
vpgl_perspective_camera<double> initCamera;
vpgl_perspective_camera<double> finalCamera;
bool isInit = VpglPlus::init_calib(worldPts, imagePts, pp, initCamera);
if (!isInit) {
printf("initiate camera error\n");
continue;
}
bool isFinal = VpglPlus::optimize_perspective_camera(worldPts, imagePts, initCamera, finalCamera);
if (!isFinal) {
printf("final camera error\n");
continue;
}
//
double pan = 0;
double tilt = 0;
double zoom = 0;
bool isPTZ = VxlPTZCamera::CameraToPTZ(finalCamera, pan, tilt, zoom);
assert(isPTZ);
observedPans.push_back(pan);
observedTilts.push_back(tilt);
observedZooms.push_back(zoom);
}
assert(gdPans.size() == observedPans.size());
assert(gdTilts.size() == observedTilts.size());
assert(gdZooms.size() == observedZooms.size());
double pan0 = gdPans[0];
const int C_M = 2; //camera state length
PanCalibrationEKF panEKF;
vnl_vector<double> x0(C_M + (int)firstWorldPts.size() * 3, 0);
x0[0] = pan0; x0[1] = 1e-10;
for (int i = 0; i<firstWorldPts.size(); i++) {
x0[C_M + 3 * i + 0] = firstWorldPts[i].x();
x0[C_M + 3 * i + 1] = firstWorldPts[i].y();
x0[C_M + 3 * i + 2] = 0.0;
}
//todo, needs to improve
vnl_matrix<double> P0(x0.size(), x0.size(), 0);
// P0(0, 0) = 0.1; P0(1, 1) = INT_MAX;
panEKF.init(x0, P0);
const int M = (int)firstWorldPts.size();
vnl_matrix<double> Q(C_M + M * 3, C_M + M * 3, 0);
Q(0, 0) = 0.0004; Q(1, 1) = 0.0001;
for (int i = 0; i<M; i++) {
Q(C_M + i, C_M + i) = 0.0;
}
vnl_matrix<double> Rk(M*2, M*2, 0);
for (int i = 0; i<M*2; i++) {
Rk(i, i) = 0.05;
}
panEKF.initNoiseCovariance(Q, Rk);
vcl_vector<vnl_vector<double> > states;
vcl_vector<double> smoothedPan;
for (int i = 0; i<observedPans.size(); i++) {
vnl_vector<double> zk(2 * M, 0); // initialize landmarks
printf("observed pan is %f\n", observedPans[i]);
// add observed point in the image space
for (int j = 0; j<M; j++) {
zk[j * 2] = observedImagePts[i][j].x();
zk[j * 2 + 1] = observedImagePts[i][j].y();
}
vnl_vector<double> xk;
vnl_matrix<double> pk;
panEKF.set_tilt_focal_length(gdTilts[i], gdZooms[i]);
panEKF.update(zk, xk, pk);
states.push_back(xk);
printf("final pan, pan_velocity is %f %f\n", xk[0], xk[1]);
printf("gd pan, is %f \n\n", gdPans[i]);
// break;
smoothedPan.push_back(xk[0]);
}
vcl_string save_file("EKF_panOnly_simulated_noise.mat");
vnl_matlab_filewrite awriter(save_file.c_str());
awriter.write(vnl_vector<double>(&observedPans[0], (int)observedPans.size()), "observed_pan");
awriter.write(vnl_vector<double>(&smoothedPan[0], (int)smoothedPan.size()), "smoothed_pan");
awriter.write(vnl_vector<double>(&gdPans[0], (int)gdPans.size()), "gdPans");
printf("save to %s\n", save_file.c_str());
}
