void restart (float ** simplex, float * response,
float * step_size)
{
const float STEP_FACTOR = 0.9;
int i, j;
int worst, next, best;
float minval, maxval;
/* find the current best vertex */
eval_vertices (response, &worst, &next, &best);
/* set the first vertex to the current best */
for (i = 0; i < DIMENSION; i++)
simplex[0][i] = simplex[best][i];
/* decrease step size */
for (i = 0; i < DIMENSION; i++)
step_size[i] *= STEP_FACTOR;
/* set up remaining vertices of simplex using new step size */
for (i = 1; i < DIMENSION+1; i++)
for (j = 0; j < DIMENSION; j++)
{
minval = simplex[0][j] - step_size[j];
maxval = simplex[0][j] + step_size[j];
simplex[i][j] = rand_uniform (minval, maxval);
}
/* initialize response for each vector */
for (i = 0; i < DIMENSION+1; i++)
response[i] = calc_error (simplex[i]);
}
double h1_error(MeshFunction *sln1, MeshFunction *sln2) {
_F_
double error = calc_error(error_fn_h1, sln1, sln2);
double norm = calc_norm(norm_fn_h1, sln2);
if (norm > H3D_TINY) return error / norm;
else return error;
}
//does everything to fill in all the data
void BAMUtils::_crunch_data() {
dna();
padded_alignment();
if (get_bamread().mapped_reverse_strand()) {
reverse();
}
//shift indels to 5'
if (five_prime_justify)
left_justify();
//cip soft clipped portions of read
if (total_three_prime_ignore > 0)
remove_bases_before_soft_clipped();
int startBase = 0;
int stopBase = 0;
bool calcRegionError = bam_record.get_region_base_positions(&startBase, &stopBase);
_region_clipped = false;
if(calcRegionError) adjust_region_base_positions(&startBase, &stopBase, pad_target, &_region_clipped);
score_alignments(q_scores, calcRegionError, startBase, stopBase);
calc_error();
}
float Model::calc_rmse(const std::vector<Node> &nodes) {
float sum = 0.0;
for (const auto &node: nodes) {
float error = calc_error(node);
sum += error * error;
}
return std::sqrt(sum / nodes.size());
}
int main(int * argc, char ** argv){
initialize_unit();
while(1){
calc_error(input_data, output_data);
back_propagation();
if(check_loop())break;
}
release_unit();
}
void simplex_initialize (float * parameters, float ** simplex,
float * response, float * step_size)
{
int i, j;
int worst, next, best;
float resp;
float minval, maxval;
for (j = 0; j < DIMENSION; j++)
{
simplex[0][j] = parameters[j];
step_size[j] = 0.5 * parameters[j];
}
for (i = 1; i < DIMENSION+1; i++)
for (j = 0; j < DIMENSION; j++)
{
minval = simplex[0][j] - step_size[j];
maxval = simplex[0][j] + step_size[j];
simplex[i][j] = rand_uniform (minval, maxval);
}
for (i = 0; i < DIMENSION+1; i++)
response[i] = calc_error (simplex[i]);
for (i = 1; i < 500; i++)
{
for (j = 0; j < DIMENSION; j++)
{
minval = simplex[0][j] - step_size[j];
maxval = simplex[0][j] + step_size[j];
parameters[j] = rand_uniform (minval, maxval);
}
resp = calc_error (parameters);
eval_vertices (response, &worst, &next, &best);
if (resp < response[worst])
replace (simplex, response, worst, parameters, resp);
}
}
// function used to combine the contributions from solution components to the total error
double error_total( double (*efn)(MeshFunction*, MeshFunction*, RefMap*, RefMap*),
double (*nfn)(MeshFunction*, RefMap*),
Tuple<Solution*>& slns1,
Tuple<Solution*>& slns2 )
{
Tuple<Solution*>::iterator it1, it2;
double error = 0.0, norm = 0.0;
for (it1=slns1.begin(), it2=slns2.begin(); it1 < slns1.end(); it1++, it2++) {
assert(it2 < slns2.end());
error += sqr(calc_error(efn, *it1, *it2));
if (nfn) norm += sqr(calc_norm(nfn, *it2));
}
return (nfn ? sqrt(error/norm) : sqrt(error));
}
void driver ( int nx, int ny, int nz, long int it_max, double tol,
double xlo, double ylo, double zlo,
double xhi, double yhi, double zhi, int io_interval){
double *f, *u;
char * rb;
int i,j,k;
double secs = -1.0;
struct timespec start, finish;
/* Allocate and initialize */
f = ( double * ) malloc ( nx * ny * nz * sizeof ( double ) );
u = ( double * ) malloc ( nx * ny * nz * sizeof ( double ) );
rb = ( char * ) malloc (nx * ny * nz * sizeof ( char ) );
get_time( &start );
omp_set_num_threads(6);
#pragma omp parallel for default(none)\
shared(nx, ny, nz, u) private(i, j, k)
for ( k = 0; k < nz; k++ )
for ( j = 0; j < ny; j++ )
for ( i = 0; i < nx; i++ )
U(i,j,k) = 0.0; /* note use of array indexing macro */
/* set rhs, and exact bcs in u */
init_prob ( nx, ny, nz, f, u , xlo, ylo, zlo, xhi, yhi, zhi);
// rb has the red and black positions
rb_vector(rb, nx, ny, nz);
/* Solve the Poisson equation */
//jacobi ( nx, ny, nz, u, f, tol, it_max, xlo, ylo, zlo, xhi, yhi, zhi, io_interval );
//gauss_seidel ( nx, ny, nz, u, f, tol, it_max, xlo, ylo, zlo, xhi, yhi, zhi, io_interval, rb );
SOR( nx, ny, nz, u, f, tol, it_max, xlo, ylo, zlo, xhi, yhi, zhi, io_interval , rb);
/* Determine the error */
calc_error ( nx, ny, nz, u, f, xlo, ylo, zlo, xhi, yhi, zhi );
/* get time for initialization and iteration. */
get_time(&finish);
secs = timespec_diff(start,finish);
printf(" Total time: %15.7e seconds\n",secs);
free ( u );
free ( f );
free (rb);
return;
}
static gboolean
recolor (/* in */ double adaptive_tightness,
/* in */ int x,
/* in */ int y,
/* in */ int width,
/* in */ int height,
/* in/out */ unsigned char *bitmap,
/* in/out */ unsigned char *mask)
{
unsigned char *index, *to_index;
int error_amt, max_error;
index = &bitmap[3 * (y * width + x) ];
to_index = NULL;
error_amt = 0;
max_error = (int) (3.0 * adaptive_tightness * adaptive_tightness);
find_most_similar_neighbor (index, &to_index, &error_amt,
x, y, width, height, bitmap, mask);
/* This condition only fails if the bitmap is all the same color */
if (to_index != NULL)
{
/*
* If the difference between the two colors is too great,
* don't coalesce the feature with its neighbor(s). This prevents a
* color from turning into its complement.
*/
if (calc_error (index, to_index) > max_error)
fill (index, x, y, width, height, bitmap, mask);
else
{
fill (to_index, x, y, width, height, bitmap, mask);
return TRUE;
}
}
return FALSE;
}
void FeatureTracker::image_callback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& info_msg) {
//need pose data for each picture, need to publish a camera pose
ros::Time acquisition_time = msg->header.stamp;
geometry_msgs::PoseStamped basePose;
geometry_msgs::PoseStamped mapPose;
basePose.pose.orientation.w=1.0;
ros::Duration timeout(3);
basePose.header.frame_id="/base_link";
mapPose.header.frame_id="/map";
try {
tf_listener_.waitForTransform("/camera_1_link", "/map", acquisition_time, timeout);
tf_listener_.transformPose("/map", acquisition_time,basePose,"/camera_1_link",mapPose);
printf("pose #%d %f %f %f\n",pic_number++,mapPose.pose.position.x, mapPose.pose.position.y, tf::getYaw(mapPose.pose.orientation));
}
catch (tf::TransformException& ex) {
ROS_WARN("[map_maker] TF exception:\n%s", ex.what());
printf("[map_maker] TF exception:\n%s", ex.what());
return;
}
cam_model.fromCameraInfo(info_msg);
// printf("callback called\n");
try
{
// if you want to work with color images, change from mono8 to bgr8
if(image_rect==NULL){
image_rect = cvCloneImage(bridge.imgMsgToCv(msg, "mono8"));
last_image= cvCloneImage(bridge.imgMsgToCv(msg, "mono8"));
pyrA=cvCreateImage(cvSize(last_image->width+8,last_image->height/3.0), IPL_DEPTH_32F, 1);
pyrB=cvCloneImage(pyrA);
// printf("cloned image\n");
}
else{
//save the last image
cvCopy(image_rect,last_image);
cvCopy(bridge.imgMsgToCv(msg, "mono8"),image_rect);
// printf("copied image\n");
}
if(output_image==NULL){
output_image =cvCloneImage(image_rect);
}
if(eigImage==NULL){
eigImage =cvCloneImage(image_rect);
}
if(tempImage==NULL){
tempImage =cvCloneImage(image_rect);
}
}
catch (sensor_msgs::CvBridgeException& e)
{
ROS_ERROR("Could not convert from '%s' to 'mono8'.", msg->encoding.c_str());
return;
}
if(image_rect!=NULL) {
cvCopy(image_rect,output_image);
printf("got image\n");
track_features(mapPose);
//draw features on the image
for(int i=0;i<last_feature_count;i++){
CvPoint center=cvPoint((int)features[i].x,(int)features[i].y);
cvCircle(output_image,center,10,cvScalar(150),2);
char strbuf [10];
int n=sprintf(strbuf,"%d",current_feature_id[ i] );
std::string text=std::string(strbuf,n);
CvFont font;
cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,1,1);
cvPutText(output_image,text.c_str(),cvPoint(center.x,center.y+20),&font,cvScalar(255));
cv::Point3d tempRay;
cv::Point2d tempPoint=cv::Point2d(features[i]);
cam_model.projectPixelTo3dRay(tempPoint,tempRay);
// printf("%f x %f y %f z\n",tempRay.x,tempRay.y,tempRay.z);
}
// featureList[0].print();
//determine error gradient
int min_features=10;
// printf("ypr %f %f %f\n",yaw,pitch,roll);
cv::Point3d error_sum=calc_error(min_features,0, 0, 0);
printf("total error is : %f\n",error_sum.x);
for(int i=0;i<featureList.size();i++){
if(min_features<featureList[i].numFeatures()){
printf("\n\n\nfeature %d\n",i);
printf("mean: %f %f %f\n",featureList[i].currentMean.x, featureList[i].currentMean.y, featureList[i].currentMean.z);
}
}
// double error_up= calc_error(min_features,yalpha, 0, 0);
// printf("total up yaw error is : %f\n",error_up);
// double error_down= calc_error(min_features,-yalpha, 0, 0);
// printf("total down yaw error is : %f\n",error_down);
/*
double yaw_change=0;
if(error_up<error_sum && error_up<error_down){
yaw_change=yalpha;
}else if(error_down<error_sum && error_down<error_up){
yaw_change=-yalpha;
}else if(error_down!=error_sum&&error_sum!=error_up){
yalpha/=2;
}
error_up= calc_error(min_features,0,palpha, 0);
// printf("total up pitch error is : %f\n",error_up);
error_down= calc_error(min_features,0,-palpha, 0);
// printf("total down pitch error is : %f\n",error_down);
double pitch_change=0;
if(error_up<error_sum && error_up<error_down){
pitch_change=palpha;
}else if(error_down<error_sum && error_down<error_up){
pitch_change=-palpha;
}else if(error_down!=error_sum&&error_sum!=error_up){
//palpha/=2;
}
error_up= calc_error(min_features,0,0,ralpha);
// printf("total up roll error is : %f\n",error_up);
error_down= calc_error(min_features,0,0,-ralpha);
// printf("total down roll error is : %f\n",error_down);
double roll_change=0;
if(error_up<error_sum && error_up<error_down){
roll_change=ralpha;
}else if(error_down<error_sum && error_down<error_up){
roll_change=-ralpha;
}else if(error_down!=error_sum&&error_sum!=error_up){
ralpha/=2;
}
// yaw+=yaw_change;
// pitch+=pitch_change;
// roll+=roll_change;
*/
try{
sensor_msgs::Image output_image_cvim =*bridge.cvToImgMsg(output_image, "mono8");
output_image_cvim.header.stamp=msg->header.stamp;
analyzed_pub_.publish(output_image_cvim);
}
catch (sensor_msgs::CvBridgeException& e){
ROS_ERROR("Could not convert from '%s' to 'mono8'.", msg->encoding.c_str());
return;
}
// printf("displaying image\n");
}else{
// printf("null image_rect\n");
}
}
void simplex_optimization (float * parameters, float * sse)
{
const int MAX_ITERATIONS = 100;
const int MAX_RESTARTS = 25;
const float EXPANSION_COEF = 2.0;
const float REFLECTION_COEF = 1.0;
const float CONTRACTION_COEF = 0.5;
const float TOLERANCE = 1.0e-10;
float ** simplex = NULL;
float * centroid = NULL;
float * response = NULL;
float * step_size = NULL;
float * test1 = NULL;
float * test2 = NULL;
float resp1, resp2;
int i, worst, best, next;
int num_iter, num_restarts;
int done;
float fit;
allocate_arrays (&simplex, ¢roid, &response, &step_size, &test1, &test2);
simplex_initialize (parameters, simplex, response, step_size);
/* start loop to do simplex optimization */
num_iter = 0;
num_restarts = 0;
done = 0;
while (!done)
{
/* find the worst vertex and compute centroid of remaining simplex,
discarding the worst vertex */
eval_vertices (response, &worst, &next, &best);
calc_centroid (simplex, worst, centroid);
/* reflect the worst point through the centroid */
calc_reflection (simplex, centroid, worst,
REFLECTION_COEF, test1);
resp1 = calc_error (test1);
/* test the reflection against the best vertex and expand it if the
reflection is better. if not, keep the reflection */
if (resp1 < response[best])
{
/* try expanding */
calc_reflection (simplex, centroid, worst, EXPANSION_COEF, test2);
resp2 = calc_error (test2);
if (resp2 <= resp1) /* keep expansion */
replace (simplex, response, worst, test2, resp2);
else /* keep reflection */
replace (simplex, response, worst, test1, resp1);
}
else if (resp1 < response[next])
{
/* new response is between the best and next worst
so keep reflection */
replace (simplex, response, worst, test1, resp1);
}
else
{
/* try contraction */
if (resp1 >= response[worst])
calc_reflection (simplex, centroid, worst,
-CONTRACTION_COEF, test2);
else
calc_reflection (simplex, centroid, worst,
CONTRACTION_COEF, test2);
resp2 = calc_error (test2);
/* test the contracted response against the worst response */
if (resp2 > response[worst])
{
/* new contracted response is worse, so decrease step size
and restart */
num_iter = 0;
num_restarts += 1;
restart (simplex, response, step_size);
}
else /* keep contraction */
replace (simplex, response, worst, test2, resp2);
}
/* test to determine when to stop.
first, check the number of iterations */
num_iter += 1; /* increment iteration counter */
if (num_iter >= MAX_ITERATIONS)
{
/* restart with smaller steps */
num_iter = 0;
num_restarts += 1;
restart (simplex, response, step_size);
}
/* limit the number of restarts */
if (num_restarts == MAX_RESTARTS) done = 1;
/* compare relative standard deviation of vertex responses
against a defined tolerance limit */
fit = calc_good_fit (response);
if (fit <= TOLERANCE) done = 1;
/* if done, copy the best solution to the output array */
if (done)
{
eval_vertices (response, &worst, &next, &best);
for (i = 0; i < DIMENSION; i++)
parameters[i] = simplex[best][i];
*sse = response[best];
}
} /* while (!done) */
number_restarts = num_restarts;
deallocate_arrays (&simplex, ¢roid, &response, &step_size,
&test1, &test2);
}
static void
find_most_similar_neighbor (/* in */ unsigned char *index,
/* in/out */ unsigned char **closest_index,
/* in/out */ int *error_amt,
/* in */ int x,
/* in */ int y,
/* in */ int width,
/* in */ int height,
/* in */ unsigned char *bitmap,
/* in/out */ unsigned char *mask)
{
int x1, x2;
int temp_error;
unsigned char *value, *temp;
if (y < 0 || y >= height || mask[y * width + x] == 2)
return;
temp = &bitmap[3 * (y * width + x)];
assert (closest_index != NULL);
if (temp[0] != index[0] || temp[1] != index[1] || temp[2] != index[2])
{
value = temp;
temp_error = calc_error (index, value);
if (*closest_index == NULL || temp_error < *error_amt)
*closest_index = value, *error_amt = temp_error;
return;
}
for (x1 = x; x1 >= 0 &&
bitmap[ 3 * (y * width + x1) ] == index[0] &&
bitmap[ 3 * (y * width + x1) + 1] == index[1] &&
bitmap[ 3 * (y * width + x1) + 2] == index[2]; x1--) ;
x1++;
for (x2 = x; x2 < width &&
bitmap[ 3 * (y * width + x2) ] == index[0] &&
bitmap[ 3 * (y * width + x2) + 1] == index[1] &&
bitmap[ 3 * (y * width + x2) + 2] == index[2]; x2++) ;
x2--;
if (x1 > 0)
{
value = &bitmap[ 3 * (y * width + x1 - 1) ];
temp_error = calc_error (index, value);
if (*closest_index == NULL || temp_error < *error_amt)
*closest_index = value, *error_amt = temp_error;
}
if (x2 < width - 1)
{
value = &bitmap[ 3 * (y * width + x2 + 1) ];
temp_error = calc_error (index, value);
if (*closest_index == NULL || temp_error < *error_amt)
*closest_index = value, *error_amt = temp_error;
}
for (x = x1; x <= x2; x++)
mask[y * width + x] = 2;
for (x = x1; x <= x2; x++)
{
find_most_similar_neighbor (index, closest_index, error_amt, x, y - 1,
width, height, bitmap, mask);
find_most_similar_neighbor (index, closest_index, error_amt, x, y + 1,
width, height, bitmap, mask);
}
}
double h1_error_axisym(MeshFunction* sln1, MeshFunction* sln2)
{
double error = calc_error(error_fn_h1_axisym, sln1, sln2);
double norm = calc_norm(norm_fn_h1_axisym, sln2);
return error/norm;
}
