int
main() {
void* jarHandle;
int error;
char* entryData = NULL;
long entryLen;
initializeStorage(".");
/* open for read binary. */
jarHandle = midpOpenJar(&error, &JAR_FILENAME);
if (error != 0) {
printf("open error = %d\n", error);
puts("Test failed");
return -1;
}
error = midpJarEntryExists(jarHandle, &UNKNOWN_ENTRY_NAME);
if (error != 0) {
printf("Jar entry was not found error = %d\n", error);
puts("Test failed");
return -1;
}
error = midpJarEntryExists(jarHandle, &ENTRY_NAME);
if (error <= 0) {
printf("Jar entry was not found error = %d\n", error);
puts("Test failed");
return -1;
}
entryLen = midpGetJarEntry(jarHandle, &UNKNOWN_ENTRY_NAME, &entryData);
if (entryLen != 0 || entryData != NULL) {
printf("Jar entry error = %ld, %d\n", entryLen, entryData);
puts("Test failed");
return -1;
}
entryLen = midpGetJarEntry(jarHandle, &ENTRY_NAME, &entryData);
if (entryLen < 0) {
printf("Jar entry error = %ld\n", entryLen);
puts("Test failed");
return -1;
}
if (entryData == NULL) {
puts("Jar entry was not found");
puts("Test failed");
return -1;
}
printf("%*.*s\n", (int)entryLen, (int)entryLen, entryData);
midpFree(entryData);
puts("Test passed");
return 0;
}
void FlowClusteringEPG::_compute()
{
if(!optflow_otl_->pull().prev_pcd_indices_) {
edge_otl_.push(NULL);
return;
}
cv::Mat3b img = optflow_otl_->pull().img_;
initializeStorage(img.rows * img.cols, 0.10);
if(cluster_index_.rows == 0)
cluster_index_ = cv::Mat1i(img.size(), -1);
const vector<int>& prev_pcd_indices = *optflow_otl_->pull().prev_pcd_indices_;
const vector<int>& pcd_indices = *optflow_otl_->pull().pcd_indices_;
const KinectCloud& prev_cloud = *index_otl_->pull().previous_pcd_;
const KinectCloud& cloud = *index_otl_->pull().current_pcd_;
ROS_ASSERT(prev_pcd_indices.size() == pcd_indices.size());
int iter = 0;
int max_iter = 500; // TODO: Parameterize.
vector<int> prev_sample(3);
vector<int> sample(3);
vector<bool> active(pcd_indices.size(), true);
while(iter < max_iter) {
++iter;
sampleFlows(active, prev_pcd_indices, pcd_indices, &prev_sample, &sample);
Eigen::Matrix4f transform;
pcl::registration::TransformationEstimationSVD<Point, Point> te;
te.estimateRigidTransformation(prev_cloud, prev_sample, cloud, sample, transform);
// -- See if this transform made any sense at all.
size_t num_inliers = 0;
for(size_t i = 0; i < sample.size(); ++i) {
Vector3f projected = (transform * prev_cloud[prev_sample[i]].getVector4fMap()).head(3);
float dist = (projected - cloud[sample[i]].getVector3fMap()).norm();
if(dist < inlier_thresh_)
++num_inliers;
}
if(num_inliers != 3)
continue;
// -- Refine the estimate.
// -- Get inlier flows.
vector<bool> inliers(pcd_indices.size(), false);
num_inliers = 0;
for(size_t i = 0; i < pcd_indices.size(); ++i) {
if(!active[i])
continue;
Vector3f projected = (transform * prev_cloud[prev_pcd_indices[i]].getVector4fMap()).head(3);
float dist = (projected - cloud[pcd_indices[i]].getVector3fMap()).norm();
if(dist < inlier_thresh_) {
inliers[i] = true;
active[i] = false;
++num_inliers;
}
}
//cout << "FC: Found cluster with " << num_inliers << " inliers out of " << pcd_indices.size() << endl;
// -- Link inliers with edge potentials.
clusters_.push_back(vector<int>());
vector<int>& cluster = clusters_.back();
cluster.reserve(num_inliers);
size_t id = clusters_.size() - 1;
for(size_t i = 0; i < inliers.size(); ++i) {
if(!inliers[i])
continue;
cluster.push_back(pcd_indices[i]); // Assumes ordered ptcld.
// Assumes ordered point cloud.
ROS_ASSERT((size_t)img.cols == prev_cloud.width);
int y = prev_pcd_indices[i] / img.cols;
int x = prev_pcd_indices[i] - y * img.cols;
cluster_index_(y, x) = id;
}
sort(cluster.begin(), cluster.end());
// Make a note if this is the one to ignore.
if(num_inliers > max_cluster_size_) {
max_cluster_size_ = num_inliers;
max_cluster_ = id;
}
// -- Break if few active flows remaining.
int num_active = 0;
for(size_t i = 0; i < active.size(); ++i)
if(active[i])
++num_active;
if(num_active < 3)
break;
}
//cout << "FC: total clusters = " << clusters_.size() << endl;
// -- potentials_ is a sparse matrix, so add links in order.
// cluster_index_(y, x) is the id of the cluster this pixel is in.
// clusters_[i] is a sorted (ascending) vector of indices into the image.
for(int y = 0; y < cluster_index_.rows; ++y) {
for(int x = 0; x < cluster_index_.cols; ++x) {
int idx0 = y * cluster_index_.cols + x;
potentials_.startVec(idx0);
if(cluster_index_(y, x) == -1)
continue;
// Ignore the biggest cluster.
if(cluster_index_(y, x) == (int)max_cluster_)
continue;
// TODO: Gross.
const vector<int>& cluster = clusters_[cluster_index_(y, x)];
if(cluster.size() < 5) {
for(size_t i = 0; i < cluster.size(); ++i) {
int idx1 = cluster[i];
if(i > 0) {
ROS_ASSERT(cluster[i] > cluster[i-1]);
//cout << "FC: Adding edge between " << idx0 << " and " << idx1 << endl;
}
if(idx0 != idx1)
potentials_.insertBack(idx0, idx1) = 1.0;
}
}
else {
for(int i = 0; i < 1; ++i) {
int r = rand() % cluster.size();
int idx1 = cluster[r];
//cout << "FC: Adding edge between " << idx0 << " and " << idx1 << endl;
if(idx0 != idx1)
potentials_.insertBack(idx0, idx1) = 1.0;
}
}
}
}
potentials_.finalize();
edge_otl_.push(&potentials_);
}
// fp1,fp2,fp3 are three training files, fin is the file to be quantized, fout is the file after quantization
void Lloyd_Max_SQ(FILE * fp1, FILE * fp2, FILE * fp3, FILE * fin, FILE * fout)
{
// basic definition
unsigned char buf;
int i;
int low_index = 0;
int high_index = SYMBOL_NUM;
int iter_count = 0;
unsigned int M = pow(2, BIT_RANGE);
unsigned char b_bound[M + 1];
unsigned char x_value[M];
unsigned int pixelString[SYMBOL_NUM];
double pixelPDF[SYMBOL_NUM];
double previous_mse = 0.0;
double current_mse;
double seta;
// initialization of the symbol stroing string
initializeStorage(pixelString);
// Read the symbols from file and store their appearances into string
readFileIntoSymbol(fp1, pixelString);
readFileIntoSymbol(fp2, pixelString);
readFileIntoSymbol(fp3, pixelString);
// convert the symbol string counts to pdf
calculatePDF(pixelString, pixelPDF);
// set lower bound and upper bound
while (pixelString[low_index] == 0) {
low_index ++;
}
while (pixelString[high_index - 1] == 0) {
high_index --;
}
b_bound[0] = low_index;
b_bound[M] = high_index - 1;
// Initialization of the M representation points x1,x2,x3...xM and M+1 boundaries b0,b1,b2...bM
initializeBoundUniform(M, x_value, b_bound, pixelPDF);
compactCode(b_bound, pixelString, M);
// Do the iteration to find suitable quantization values
while (iter_count < MAX_ITERATION){
iter_count ++;
printf("In the %d th iteration.\n",iter_count);
// for (int i = 0; i < M; i ++) {
// printf("x[%d] = %d\n",i,x_value[i]);
// }
// for (int i = 0 ; i < M + 1; i++) {
// printf("b[%d] = %d\n",i,b_bound[i]);
// }
for (i = 1; i < M; i ++) {
b_bound[i] = (x_value[i - 1]+x_value[i] + 1)/2;
}
compactCode(b_bound, pixelString, M);
for (i = 0; i < M; i ++) {
x_value[i] = conditionalMean(b_bound[i], b_bound[i + 1], pixelPDF);
}
current_mse = calculateMse(M, x_value, b_bound, pixelString, pixelPDF);
seta = (previous_mse - current_mse)/current_mse;
if ( seta <= ERROR_THRESHOLD && seta >= - ERROR_THRESHOLD){
break;
}
previous_mse = current_mse;
}
printf("It takes %d iterations to converge.\n",iter_count);
// Quantize the input images
fseek(fin,0,SEEK_SET); // point the file handle back to the begining of the file
calculatePSNRforImage(b_bound, x_value, M, fin);
fseek(fin,0,SEEK_SET); // point the file handle back to the begining of the file
while (fread(&buf, sizeof(unsigned char), 1, fin)) {
buf = findQuantizedValue(buf, b_bound, x_value, M);
fwrite(&buf, sizeof(unsigned char), 1, fout);
}
}
