int
main(int argc, char *argv[])
{
static AdData data; /* to be init with 0 (debug only) */
AdData *p_ad = &data;
int i, user_stat = 0;
double time_one0, time_one;
double nb_same_var_by_iter, nb_same_var_by_iter_tot;
int nb_iter_cum;
int nb_local_min_cum;
int nb_swap_cum;
int nb_reset_cum;
double nb_same_var_by_iter_cum;
int nb_restart_cum, nb_restart_min, nb_restart_max;
double time_cum, time_min, time_max;
int nb_iter_tot_cum, nb_iter_tot_min, nb_iter_tot_max;
int nb_local_min_tot_cum, nb_local_min_tot_min, nb_local_min_tot_max;
int nb_swap_tot_cum, nb_swap_tot_min, nb_swap_tot_max;
int nb_reset_tot_cum, nb_reset_tot_min, nb_reset_tot_max;
double nb_same_var_by_iter_tot_cum, nb_same_var_by_iter_tot_min, nb_same_var_by_iter_tot_max;
int user_stat_cum, user_stat_min, user_stat_max;
char buff[256], str[32];
/* Seeds generation */
int last_value; /* last value generated by the linear chaotic map */
int param_a; /* parameter 'a' for the linear chaotic map */
int param_c; /* parameter 'c' for the linear chaotic map */
long int print_seed ;
struct timeval tv ;
#if defined PRINT_COSTS
char * tmp_filename=NULL ;
#endif /* PRINT_COSTS */
#if defined MPI
Main_MPIData mpi_data ;
#endif
Parse_Cmd_Line(argc, argv, p_ad);
/************************ Initialize chaotic function **********************/
param_a = 5; /* Values by default from research paper */
param_c = 1;
gettimeofday(&tv, NULL);
/*********************** MPI & SEQ code Initialization *********************/
#if defined MPI
mpi_data.param_a_ptr = ¶m_a ;
mpi_data.param_c_ptr = ¶m_c ;
mpi_data.last_value_ptr = &last_value ;
mpi_data.p_ad = p_ad ;
mpi_data.p_ad->main_mpi_data_ptr = &mpi_data ;
mpi_data.print_seed_ptr = &print_seed ;
mpi_data.tv_sec = tv.tv_sec ;
mpi_data.count_ptr = &count ;
#if defined STATS
Gmpi_stats.nb_sent_messages = 0 ;
Gmpi_stats.nb_sent_mymessages = 0 ;
#endif
#if defined PRINT_COSTS
mpi_data.nb_digits_nbprocs_ptr = &nb_digits_nbprocs ;
#endif
MPI_Init( &argc , &argv ) ;
MPI_Comm_rank(MPI_COMM_WORLD, &my_num) ;
MPI_Comm_size(MPI_COMM_WORLD, &mpi_size) ;
TPRINT0("Program: %s", argv[0]) ;
for( i=1 ; i< argc ; ++i )
PRINT0(" %s", argv[i]) ;
PRINT0("\n") ;
AS_MPI_initialization( &mpi_data ) ;
#else /****************************** MPI -> SEQ *****************************/
TPRINT0("Program: %s", argv[0]) ;
for( i=1 ; i< argc ; i++ )
PRINT0(" %s", argv[i]) ;
PRINT0("\n") ;
#if defined PRINT_COSTS
nb_digits_nbprocs = 0 ;
#endif /* PRINT_COSTS */
if (p_ad->seed < 0) {
srandom((unsigned int)tv.tv_sec);
/* INT_MAX / 6 (= 357.913.941) is a reasonable value to start with...
I think... */
last_value = (int) Random(INT_MAX / 6);
/* INT_MAX (= 2.147.483.647) is the max value for a signed 32-bit int */
p_ad->seed = randChaos(INT_MAX, &last_value, ¶m_a, ¶m_c);
}
print_seed = p_ad->seed;
#endif /**************************** MPI */
#if defined PRINT_COSTS
if( filename_pattern_print_cost==NULL ) {
PRINT0("Please give a pattern for filename in which to save costs\n\n") ;
exit(-1) ;
}
tmp_filename=(char*)
malloc(sizeof(char)*strlen(filename_pattern_print_cost)
+ 2
+ nb_digits_nbprocs ) ;
if( nb_digits_nbprocs > 3 ) {
PRINT0("You use a number of procs sup to 999. "
"You must modify the code to adjust next line\n") ;
exit(-1) ;
}
#if defined MPI
/* TODO: How can we bypass the static char to format output in nxt line? */
sprintf(tmp_filename,"%s_p%03d", filename_pattern_print_cost,my_num) ;
#else
sprintf(tmp_filename,"%s_seq", filename_pattern_print_cost) ;
#endif
file_descriptor_print_cost = open(tmp_filename,
O_WRONLY | O_EXCL | O_CREAT,
S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP) ;
if( file_descriptor_print_cost == -1 ) {
PRINTF("Cannot create file to print costs: file already exists?\n") ;
return -1 ;
}
#endif /* PRINT_COSTS */
/********* Initialization of the pseudo-random generator with the seed ******/
srandom((unsigned)p_ad->seed);
p_ad->nb_var_to_reset = -1;
p_ad->do_not_init = 0;
p_ad->actual_value = NULL;
p_ad->base_value = 0;
p_ad->break_nl = 0;
/* defaults */
Init_Parameters(p_ad);
if (p_ad->reset_limit >= p_ad->size)
p_ad->reset_limit = p_ad->size - 1;
setvbuf(stdout, NULL, _IOLBF, 0);
//setlinebuf(stdout);
if (p_ad->debug > 0 && !ad_has_debug)
DPRINT0("Warning ad_solver is not compiled with debugging support\n") ;
if (p_ad->log_file && !ad_has_log_file)
DPRINT0("Warning ad_solver is not compiled with log file support\n") ;
p_ad->size_in_bytes = p_ad->size * sizeof(int);
p_ad->sol = malloc(p_ad->size_in_bytes);
if (p_ad->nb_var_to_reset == -1) {
p_ad->nb_var_to_reset = Div_Round_Up(p_ad->size * p_ad->reset_percent, 100);
if (p_ad->nb_var_to_reset < 2) {
p_ad->nb_var_to_reset = 2;
PRINT0("increasing nb var to reset since too small, now = %d\n",
p_ad->nb_var_to_reset);
}
}
#if defined MPI
AS_MPI_initialization_epilogue( &mpi_data ) ;
#endif
/********** Print configuration information + specific initialization *****/
PRINT0("current random seed used: %u (seed_0 %u) \n",
(unsigned int)p_ad->seed, (unsigned int)print_seed) ;
PRINT0("variables of loc min are frozen for: %d swaps\n",
p_ad->freeze_loc_min) ;
PRINT0("variables swapped are frozen for: %d swaps\n", p_ad->freeze_swap) ;
if (p_ad->reset_percent >= 0)
PRINT0("%d %% = ", p_ad->reset_percent) ;
PRINT0("%d variables are reset when %d variables are frozen\n",
p_ad->nb_var_to_reset, p_ad->reset_limit) ;
PRINT0("probability to select a local min (instead of "
"staying on a plateau): ") ;
if (p_ad->prob_select_loc_min >=0 && p_ad->prob_select_loc_min <= 100)
PRINT0("%d %%\n", p_ad->prob_select_loc_min) ;
else PRINT0("not used\n") ;
PRINT0("abort when %d iterations are reached "
"and restart at most %d times\n",
p_ad->restart_limit, p_ad->restart_max) ;
PrintAllCompilationOptions() ;
#if defined BACKTRACK
/* Note: This has to be done after p_ad initialization! */
/* Gbacktrack_array_begin = 0 ; */
/* Gbacktrack_array_end = 0 ; */
/* Gconfiguration_size_in_bytes = p_ad->size_in_bytes ; */
/* for( i=0 ; i<SIZE_BACKTRACK ; i++ ) */
/* Gbacktrack_array[i].configuration = (unsigned int*) */
/* malloc(Gconfiguration_size_in_bytes) ; */
/* YC->all: do the rest of initilization */
gl_elitePool.config_list_begin = NULL;
gl_elitePool.config_list_end = NULL;
gl_elitePool.config_list_size = 0;
gl_elitePool.nb_backtrack = 0;
gl_elitePool.nb_variable_backtrack = 0;
gl_elitePool.nb_value_backtrack = 0;
gl_stockPool.config_list_begin = NULL;
gl_stockPool.config_list_end = NULL;
gl_stockPool.config_list_size = 0;
backtrack_configuration *item;
for (i = 0; i < SIZE_BACKTRACK; i++)
{
item = malloc(sizeof(backtrack_configuration));
item->configuration = malloc(p_ad->size_in_bytes);
pushStock(item);
}
#endif
TPRINT0("%d begins its resolution!\n", my_num) ;
TPRINT0("count = %d\n", count);
if (count <= 0) /* Note: MPI => count=1 */
{
Set_Initial(p_ad);
time_one0 = (double) User_Time();
Solve(p_ad);
time_one = ((double) User_Time() - time_one0) / 1000;
if (p_ad->exhaustive)
DPRINTF("exhaustive search\n") ;
if (count < 0)
Display_Solution(p_ad);
Verify_Sol(p_ad);
if (p_ad->total_cost)
PRINTF("*** NOT SOLVED (cost of this pseudo-solution: %d) ***\n", p_ad->total_cost) ;
if (count == 0)
{
nb_same_var_by_iter = (double) p_ad->nb_same_var / p_ad->nb_iter;
nb_same_var_by_iter_tot = (double) p_ad->nb_same_var_tot / p_ad->nb_iter_tot;
PRINTF("%5d %8.2f %8d %8d %8d %8d %8.1f %8d %8d %8d %8d %8.1f",
p_ad->nb_restart, time_one,
p_ad->nb_iter, p_ad->nb_local_min, p_ad->nb_swap,
p_ad->nb_reset, nb_same_var_by_iter,
p_ad->nb_iter_tot, p_ad->nb_local_min_tot, p_ad->nb_swap_tot,
p_ad->nb_reset_tot, nb_same_var_by_iter_tot);
if (user_stat_fct)
PRINTF(" %8d", (*user_stat_fct)(p_ad));
PRINTF("\n");
}
else
{
PRINTF("in %.2f secs (%d restarts, %d iters, %d loc min, %d swaps, %d resets)\n",
time_one, p_ad->nb_restart, p_ad->nb_iter_tot, p_ad->nb_local_min_tot,
p_ad->nb_swap_tot, p_ad->nb_reset_tot);
}
#if defined BACKTRACK
PRINTF("BACKTRACK STATS:\n");
PRINTF("Total number of performed backtracks: %d\n", gl_elitePool.nb_backtrack);
PRINTF("Total number of performed backtracks starting from another variable: %d\n", gl_elitePool.nb_variable_backtrack);
PRINTF("Total number of performed backtracks starting from another value: %d\n", gl_elitePool.nb_value_backtrack);
#endif
#if defined STATS
print_stats() ;
#endif
return 0 ;
} /* (count <= 0) */
if (user_stat_name)
sprintf(str, " %8s |", user_stat_name);
else
*str = '\0';
snprintf(buff, 255,
"|Count|restart| time | iters | loc min | swaps "
"| resets | same/iter|%s\n", str);
if (param_needed > 0)
PRINT0("%*d\n", (int) strlen(buff)/2, p_ad->param) ;
else if (param_needed < 0)
PRINT0("%*s\n", (int) strlen(buff)/2, p_ad->param_file) ;
PRINT0("%s", buff) ;
for(i = 0; buff[i] != '\n'; ++i)
if (buff[i] != '|')
buff[i] = '-';
PRINT0("%s\n", buff) ;
nb_restart_cum = time_cum = user_stat_cum = 0;
nb_iter_cum = nb_local_min_cum = nb_swap_cum = nb_reset_cum = 0;
nb_same_var_by_iter_cum = user_stat_cum = 0;
nb_iter_tot_cum = nb_local_min_tot_cum = nb_swap_tot_cum = nb_reset_tot_cum = 0;
nb_same_var_by_iter_tot_cum = 0;
nb_restart_min = user_stat_min = (1 << 30);
time_min = 1e100;
nb_iter_tot_min = nb_local_min_tot_min = nb_swap_tot_min = nb_reset_tot_min = (1 << 30);
nb_same_var_by_iter_tot_min = 1e100;
nb_restart_max = user_stat_max = 0;
time_max = 0;
nb_iter_tot_max = nb_local_min_tot_max = nb_swap_tot_max = nb_reset_tot_max = 0;
nb_same_var_by_iter_tot_max = 0;
for(i = 1; i <= count; i++)
{
Set_Initial(p_ad);
time_one0 = (double) User_Time();
Solve(p_ad);
time_one = ((double) User_Time() - time_one0) / 1000;
#if !defined MPI /* Slashes MPI output! */
if (disp_mode == 2 && nb_restart_cum > 0)
PRINTF("\033[A\033[K");
PRINTF("\033[A\033[K\033[A\033[256D");
#endif
Verify_Sol(p_ad);
if (user_stat_fct)
user_stat = (*user_stat_fct)(p_ad);
nb_same_var_by_iter = (double) p_ad->nb_same_var / p_ad->nb_iter;
nb_same_var_by_iter_tot = (double) p_ad->nb_same_var_tot / p_ad->nb_iter_tot;
nb_restart_cum += p_ad->nb_restart;
time_cum += time_one;
nb_iter_cum += p_ad->nb_iter;
nb_local_min_cum += p_ad->nb_local_min;
nb_swap_cum += p_ad->nb_swap;
nb_reset_cum += p_ad->nb_reset;
nb_same_var_by_iter_cum += nb_same_var_by_iter;
user_stat_cum += user_stat;
nb_iter_tot_cum += p_ad->nb_iter_tot;
nb_local_min_tot_cum += p_ad->nb_local_min_tot;
nb_swap_tot_cum += p_ad->nb_swap_tot;
nb_reset_tot_cum += p_ad->nb_reset_tot;
nb_same_var_by_iter_tot_cum += nb_same_var_by_iter_tot;
if (nb_restart_min > p_ad->nb_restart)
nb_restart_min = p_ad->nb_restart;
if (time_min > time_one)
time_min = time_one;
if (nb_iter_tot_min > p_ad->nb_iter_tot)
nb_iter_tot_min = p_ad->nb_iter_tot;
if (nb_local_min_tot_min > p_ad->nb_local_min_tot)
nb_local_min_tot_min = p_ad->nb_local_min_tot;
if (nb_swap_tot_min > p_ad->nb_swap_tot)
nb_swap_tot_min = p_ad->nb_swap_tot;
if (nb_reset_tot_min > p_ad->nb_reset_tot)
nb_reset_tot_min = p_ad->nb_reset_tot;
if (nb_same_var_by_iter_tot_min > nb_same_var_by_iter_tot)
nb_same_var_by_iter_tot_min = nb_same_var_by_iter_tot;
if (user_stat_min > user_stat)
user_stat_min = user_stat;
if (nb_restart_max < p_ad->nb_restart)
nb_restart_max = p_ad->nb_restart;
if (time_max < time_one)
time_max = time_one;
if (nb_iter_tot_max < p_ad->nb_iter_tot)
nb_iter_tot_max = p_ad->nb_iter_tot;
if (nb_local_min_tot_max < p_ad->nb_local_min_tot)
nb_local_min_tot_max = p_ad->nb_local_min_tot;
if (nb_swap_tot_max < p_ad->nb_swap_tot)
nb_swap_tot_max = p_ad->nb_swap_tot;
if (nb_reset_tot_max < p_ad->nb_reset_tot)
nb_reset_tot_max = p_ad->nb_reset_tot;
if (nb_same_var_by_iter_tot_max < nb_same_var_by_iter_tot)
nb_same_var_by_iter_tot_max = nb_same_var_by_iter_tot;
if (user_stat_max < user_stat)
user_stat_max = user_stat;
#if !defined(MPI)
switch(disp_mode)
{
case 0: /* only last iter counters */
case 2: /* last iter followed by restart if needed */
PRINTF("|%4d | %5d%c| %8.2f | %8d | %8d | %8d | %8d | %8.1f |",
i, p_ad->nb_restart, (p_ad->total_cost == 0) ? ' ' : 'N',
time_one,
p_ad->nb_iter, p_ad->nb_local_min, p_ad->nb_swap,
p_ad->nb_reset, nb_same_var_by_iter);
if (user_stat_fct)
PRINTF(" %8d |", user_stat);
PRINTF("\n");
if (disp_mode == 2 && p_ad->nb_restart > 0)
{
PRINTF("| | | |"
" %8d | %8d | %8d | %8d | %8.1f |",
p_ad->nb_iter_tot, p_ad->nb_local_min_tot,
p_ad->nb_swap_tot,
p_ad->nb_reset_tot, nb_same_var_by_iter_tot);
if (user_stat_fct)
PRINTF(" |");
PRINTF("\n");
}
PRINTF("%s", buff);
PRINTF("| avg | %5d | %8.2f | %8d | %8d | %8d | %8d | %8.1f |",
nb_restart_cum / i, time_cum / i,
nb_iter_cum / i, nb_local_min_cum / i, nb_swap_cum / i,
nb_reset_cum / i, nb_same_var_by_iter_cum / i);
if (user_stat_fct)
PRINTF(" %8.2f |", (double) user_stat_cum / i);
PRINTF("\n");
if (disp_mode == 2 && nb_restart_cum > 0)
{
PRINTF("| | | |"
" %8d | %8d | %8d | %8d | %8.1f |",
nb_iter_tot_cum / i, nb_local_min_tot_cum / i,
nb_swap_tot_cum / i,
nb_reset_tot_cum / i, nb_same_var_by_iter_tot_cum / i);
if (user_stat_fct)
PRINTF(" |");
PRINTF("\n");
}
break;
case 1: /* only total (restart + last iter) counters */
PRINTF("|%4d | %5d%c| %8.2f | %8d | %8d | %8d | %8d | %8.1f |",
i, p_ad->nb_restart, (p_ad->total_cost == 0) ? ' ' : 'N',
time_one,
p_ad->nb_iter_tot, p_ad->nb_local_min_tot, p_ad->nb_swap_tot,
p_ad->nb_reset_tot, nb_same_var_by_iter_tot);
if (user_stat_fct)
PRINTF(" %8d |", user_stat);
PRINTF("\n");
PRINTF("%s", buff);
PRINTF("| avg | %5d | %8.2f | %8d | %8d | %8d | %8d | %8.1f |",
nb_restart_cum / i, time_cum / i,
nb_iter_tot_cum / i, nb_local_min_tot_cum / i,
nb_swap_tot_cum / i,
nb_reset_tot_cum / i, nb_same_var_by_iter_tot_cum / i);
if (user_stat_fct)
PRINTF(" %8.2f |", (double) user_stat_cum / i);
PRINTF("\n");
break;
}
#else /* MPI */
/* disp_mode equals 1 by default */
/* Prepare what will be sent to 0, and/or printed by 0 */
snprintf(mpi_data.results, RESULTS_CHAR_MSG_SIZE - 1,
"|* %ld/(%d/%d) | %5d | %8.2f | %8d | %8d | %8d | %8d | %8.1f |",
print_seed,
my_num,
mpi_size,
nb_restart_cum / i, time_cum / i,
nb_iter_tot_cum / i, nb_local_min_tot_cum / i,
nb_swap_tot_cum / i,
nb_reset_tot_cum / i, nb_same_var_by_iter_tot_cum / i);
/* TODO: use if(user_stat_fct)? What is that? */
#endif /* MPI */
} /* for(i = 1; i <= count; i++) */
if (count <= 0) /* YC->DD: is it really possible here? return 0 before.*/
return 0;
if( count > 1 ) { /* YC->DD: why this test has been removed? */
PRINTF("| min | %5d | %8.2f | %8d | %8d | %8d | %8d | %8.1f |",
nb_restart_min, time_min,
nb_iter_tot_min, nb_local_min_tot_min, nb_swap_tot_min,
nb_reset_tot_min, nb_same_var_by_iter_tot_min);
if (user_stat_fct)
PRINTF(" %8d |", user_stat_min);
PRINTF("\n");
PRINTF("| max | %5d | %8.2f | %8d | %8d | %8d | %8d | %8.1f |",
nb_restart_max, time_max,
nb_iter_tot_max, nb_local_min_tot_max, nb_swap_tot_max,
nb_reset_tot_max, nb_same_var_by_iter_tot_max);
if (user_stat_fct)
PRINTF(" %8d |", user_stat_max);
PRINTF("\n");
}
#if defined BACKTRACK
PRINTF("BACKTRACK STATS:\n");
PRINTF("Total number of performed backtracks: %d\n", gl_elitePool.nb_backtrack);
PRINTF("Total number of performed backtracks starting from another variable: %d\n", gl_elitePool.nb_variable_backtrack);
PRINTF("Total number of performed backtracks starting from another value: %d\n", gl_elitePool.nb_value_backtrack);
/* flush pools */
while(gl_elitePool.config_list_size > 0)
{
item = popElite();
free(item->configuration);
free(item);
}
while(gl_stockPool.config_list_size > 0)
{
item = popStock();
free(item->configuration);
free(item);
}
#endif /* BACKTRACK */
#if defined STATS
print_stats() ;
#endif
#if !( defined MPI )
#if defined PRINT_COSTS
print_costs() ;
#endif
TDPRINTF("Processus ends now.\n") ; /* Seq code is now ending */
#else /* !( defined MPI) */
AS_MPI_completion( &mpi_data ) ;
#endif /* MPI */
}
void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
{
b2Timer stepTimer;
// If new fixtures were added, we need to find the new contacts.
if (m_flags & e_newFixture)
{
m_contactManager.FindNewContacts();
m_flags &= ~e_newFixture;
}
m_flags |= e_locked;
b2TimeStep step;
step.dt = dt;
step.velocityIterations = velocityIterations;
step.positionIterations = positionIterations;
if (dt > 0.0f)
{
step.inv_dt = 1.0f / dt;
}
else
{
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * dt;
step.warmStarting = m_warmStarting;
// Update contacts. This is where some contacts are destroyed.
{
b2Timer timer;
m_contactManager.Collide();
m_profile.collide = timer.GetMilliseconds();
}
// Integrate velocities, solve velocity constraints, and integrate positions.
if (m_stepComplete && step.dt > 0.0f)
{
b2Timer timer;
Solve(step);
m_profile.solve = timer.GetMilliseconds();
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
b2Timer timer;
SolveTOI(step);
m_profile.solveTOI = timer.GetMilliseconds();
}
if (step.dt > 0.0f)
{
m_inv_dt0 = step.inv_dt;
}
if (m_flags & e_clearForces)
{
ClearForces();
}
m_flags &= ~e_locked;
m_profile.step = stepTimer.GetMilliseconds();
}
int main(int argc, char *argv[])
{
if (argc != 2)
{
fprintf(stderr, "Usage: %s port\n", argv[0]);
exit(EXIT_FAILURE);
}
int pid = fork();
if(pid)
{
waitpid(pid, NULL, 0);
}
else
{
pid_t sid = setsid();
if (sid == (pid_t) -1)
{
perror("setsid");
_exit(EXIT_FAILURE);
}
AddrInit();
SetConnection(argv[1]);
struct pollfd ufds[MAX_CLIENTS + 1];
int ufdsCount = 1;
ufds[0].fd = sfd;
ufds[0].events = POLLIN;
gamesCount = 0;
int curGame = 0;
int rv;
for (;;) {
rv = poll(ufds, ufdsCount + 1, -1);
if (rv == -1)
{
printf("poll Error");
perror("poll"); // error occurred in poll()
}
else
{
if (rv > 0)
{
// check for events on soket ( adding new clients)
if (ufds[0].revents & POLLIN)
{
int new_cfd; // client fd
struct sockaddr_storage peer_addr;
socklen_t addr_size = sizeof(peer_addr);
if ( (new_cfd = accept(sfd, (struct sockaddr *) &peer_addr, &addr_size)) == -1)
{
if(close(sfd) == -1) { _exit(EXIT_FAILURE);}
perror("accept fail");
return 6;
}
if (new_cfd < MAX_CLIENTS )
{
ufds[new_cfd].fd = new_cfd;
ufds[new_cfd].events = POLLIN | POLLRDHUP; // check for just normal data
ufdsCount = new_cfd;
printf("creating new games[%d]\n", new_cfd);
games[new_cfd].game_state = 0;
}
else
{
printf("New client dismist\n");
if(close(new_cfd) == -1) { _exit(EXIT_FAILURE);}
continue;
}
//write_to_client(new_cfd, "cenected to server.\n");
printf("New client %d\n", new_cfd);
int pair_player;
if(new_cfd % 2 == 0)
{
pair_player = new_cfd + 1;
games[new_cfd].setMesToBuf("Please wait for second player.\n");
ufds[new_cfd].events |= POLLOUT;
continue;
}
else
{
pair_player = new_cfd - 1;
}
games[new_cfd].game_state = 1;
games[pair_player].game_state = 1;
games[new_cfd].setMesToBuf("Ready to play.\nYour tearn.\n");
games[pair_player].setMesToBuf("Ready to play.\nYour tearn.\n");
ufds[new_cfd].events |= POLLOUT;
ufds[pair_player].events |= POLLOUT;
continue;
}
int i;
char mes = ' ';
for (i = 1; i <= ufdsCount; ++i)
{
if (ufds[i].revents & POLLRDHUP)
{
printf("POLLRDHUP ivent\n");
int pair_player;
if(i % 2 == 0)
{
pair_player = i + 1;
}
else
{
pair_player = i - 1;
}
if(close(i) == -1) { _exit(EXIT_FAILURE);}
if(close(pair_player) == -1) { _exit(EXIT_FAILURE);}
//games[pair_player].setMesToBuf("You partner is off, game over.\n");
ufds[i].fd = -1;
ufds[i].events = 0;
ufds[pair_player].fd = -1;
ufds[pair_player].events = 0;
games.erase(i);
games.erase(pair_player);
printf("Game %d end %d is over. One of the players is off.\n", i, pair_player);
continue;
}
if (ufds[i].revents & POLLOUT)
{
//printf("POLLOUT ivent\n");
if (games[i].len_out != 0)
{
int writen = write(ufds[i].fd, games[i].buf_out, games[i].len_out);
//int writen = write(ufds[i].fd, "meage\n", 6);
if (writen == -1)
{
printf("Write error\n");
//handle_error("write", ufds, inputs, i, &nfds);
}
else
{
games[i].len_out -= writen;
printf("write game[%d].buf_out:%s, %zu\n", i, games[i].buf_out, games[i].len_out);
if (games[i].len_out == 0) {
ufds[i].events = POLLIN | POLLRDHUP;
}
}
}
}
if (ufds[i].revents & POLLIN)
{
printf("POLLIN ivent\n");
nread = read(ufds[i].fd, games[i].buf_in + games[i].len_in, BUF_SIZE - games[i].len_in);
if (nread == -1)
{
printf("reading tearn error\n");
continue; /* Ignore failed request */
}
if (games[i].len_in + nread < BUF_SIZE)
{
games[i].len_in += nread;
}
else
{
printf("reading from client error, to long mes\n");
continue;
}
printf("Received %zu bytes from soket %d :%s\n", nread, ufds[i].fd, games[i].buf_in);
if(games[i].game_state == 1)
{
if (games[i].state == 0)
{
printf("Player%d did tearn: %c\n", i, games[i].mes);
games[i].handleIncomingData();
games[i].setMesToBuf("Your turn ecepted.\n");
ufds[i].events |= POLLOUT;
}
else
{
printf("Player%d did extra tearn: %c\n", i, games[i].mes);
games[i].len_in = 0;
games[i].setMesToBuf("Wait till your tern.\n");
ufds[i].events |= POLLOUT;
continue;
}
}
else
{
printf("Player %d has no partner.\n", i);
games[i].setMesToBuf("Wait before second plyer conected.\n");
ufds[i].events |= POLLOUT;
games[i].len_in = 0;
continue;
}
int pair_player;
if(i % 2 == 0)
{
pair_player = i + 1;
}
else
{
pair_player = i - 1;
}
//assert( games[i].state && games[pair_player].state);
if(games[i].state == 1 && games[pair_player].state == 1)
{
printf("Got bouth tearns.\n", i);
int res = Solve(games[i].mes, games[pair_player].mes);
if(res == 0)
{
games[i].setMesToBuf("Nobody win!\n");
games[pair_player].setMesToBuf("Nobody win!\n");
ufds[i].events |= POLLOUT;
ufds[pair_player].events |= POLLOUT;
}
if(res == 1)
{
games[i].setMesToBuf("You win!\n");
games[pair_player].setMesToBuf("You lose!\n");
ufds[i].events |= POLLOUT;
ufds[pair_player].events |= POLLOUT;
}
if(res == 2)
{
games[i].setMesToBuf("You lose!\n");
games[pair_player].setMesToBuf("You win!\n");
ufds[i].events |= POLLOUT;
ufds[pair_player].events |= POLLOUT;
}
if(res == -1)
{
games[i].setMesToBuf("Wrong licsikon!\n");
games[pair_player].setMesToBuf("Wrong licsikon!\n");
ufds[i].events |= POLLOUT;
ufds[pair_player].events |= POLLOUT;
}
games[i].state = 0;
games[pair_player].state = 0;
games[i].setMesToBuf("Next game\nYour tearn!\n");
games[pair_player].setMesToBuf("Next game\nYour tearn!\n");
ufds[i].events |= POLLOUT;
ufds[pair_player].events |= POLLOUT;
printf("Game res:%d\n", res);
}
else
{
printf("Not bouth stre = 1.\n");
}
}
}
}
}
}
if(close(sfd) == -1) { _exit(EXIT_FAILURE);}
}
_exit(EXIT_SUCCESS);
}
TEST_F(EmbeddedExplicitRungeKuttaNyströmIntegratorTest, Singularity) {
// Integrating the position of an ideal rocket,
// x"(t) = m' I_sp / m(t),
// x'(0) = 0, x(0) = 0,
// where m(t) = m₀ - t m'.
// The solution is
// x(t) = I_sp (t + (t - m₀ / m') log(m₀ / m(t))
// x'(t) = I_sp log(m₀ / m(t)) (Циолко́вский's equation).
// There is a singularity at t = m₀ / m'.
Variation<Mass> const mass_flow = 1 * Kilogram / Second;
Mass const initial_mass = 1 * Kilogram;
SpecificImpulse const specific_impulse = 1 * Newton * Second / Kilogram;
Instant const t_initial;
Instant const t_singular = t_initial + initial_mass / mass_flow;
// After the singularity.
Instant const t_final = t_initial + 2 * initial_mass / mass_flow;
auto const mass = [initial_mass, t_initial, mass_flow](Instant const& t) {
return initial_mass - (t - t_initial) * mass_flow;
};
Length const length_tolerance = 1 * Milli(Metre);
Speed const speed_tolerance = 1 * Milli(Metre) / Second;
std::vector<ODE::SystemState> solution;
ODE rocket_equation;
rocket_equation.compute_acceleration = [&mass, specific_impulse, mass_flow](
Instant const& t,
std::vector<Length> const& position,
std::vector<Acceleration>& acceleration) {
acceleration.back() = mass_flow * specific_impulse / mass(t);
return Status::OK;
};
IntegrationProblem<ODE> problem;
auto const append_state = [&solution](ODE::SystemState const& state) {
solution.push_back(state);
};
problem.equation = rocket_equation;
problem.initial_state = {{0 * Metre}, {0 * Metre / Second}, t_initial};
AdaptiveStepSizeIntegrator<ODE>::Parameters const parameters(
/*first_time_step=*/t_final - t_initial,
/*safety_factor=*/0.9);
auto const tolerance_to_error_ratio = [length_tolerance, speed_tolerance](
Time const& h, ODE::SystemStateError const& error) {
return std::min(length_tolerance / Abs(error.position_error[0]),
speed_tolerance / Abs(error.velocity_error[0]));
};
AdaptiveStepSizeIntegrator<ODE> const& integrator =
EmbeddedExplicitRungeKuttaNyströmIntegrator<
methods::DormandالمكاوىPrince1986RKN434FM,
Length>();
auto const instance = integrator.NewInstance(problem,
append_state,
tolerance_to_error_ratio,
parameters);
auto const outcome = instance->Solve(t_final);
EXPECT_EQ(termination_condition::VanishingStepSize, outcome.error());
EXPECT_EQ(132, solution.size());
EXPECT_THAT(solution.back().time.value - t_initial,
AlmostEquals(t_singular - t_initial, 15));
EXPECT_THAT(solution.back().positions.back().value,
AlmostEquals(specific_impulse * initial_mass / mass_flow, 540));
}
bool QPBO<REAL>::Save(char* filename, int format)
{
int e;
int edge_num = 0;
for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e)) edge_num ++;
if (format == 0)
{
FILE* fp;
REAL E0, E1, E00, E01, E10, E11;
int i, j;
char* type_name;
char* type_format;
char FORMAT_LINE[64];
int factor = (stage == 0) ? 2 : 1;
get_type_information(type_name, type_format);
fp = fopen(filename, "w");
if (!fp) return false;
fprintf(fp, "nodes=%d\n", GetNodeNum());
fprintf(fp, "edges=%d\n", edge_num);
fprintf(fp, "labels=2\n");
fprintf(fp, "type=%s\n", type_name);
fprintf(fp, "\n");
sprintf(FORMAT_LINE, "n %%d\t%%%s %%%s\n", type_format, type_format);
for (i=0; i<GetNodeNum(); i++)
{
GetTwiceUnaryTerm(i, E0, E1);
REAL delta = (E0 < E1) ? E0 : E1;
fprintf(fp, FORMAT_LINE, i, (E0-delta)/factor, (E1-delta)/factor);
}
sprintf(FORMAT_LINE, "e %%d %%d\t%%%s %%%s %%%s %%%s\n", type_format, type_format, type_format, type_format);
for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e))
{
GetTwicePairwiseTerm(e, i, j, E00, E01, E10, E11);
fprintf(fp, FORMAT_LINE, i, j, E00/factor, E01/factor, E10/factor, E11/factor);
}
fclose(fp);
return true;
}
if (format == 1)
{
FILE* fp;
REAL E0, E1, E00, E01, E10, E11;
int i, j;
Arc* a;
char* type_name;
char* type_format;
if (stage == 0) Solve();
get_type_information(type_name, type_format);
if (type_format[0] != 'd') return false;
fp = fopen(filename, "w");
if (!fp) return false;
fprintf(fp, "p %d %d\n", GetNodeNum(), GetNodeNum() + edge_num);
for (i=0; i<GetNodeNum(); i++)
{
GetTwiceUnaryTerm(i, E0, E1);
REAL delta = E1 - E0;
for (a=nodes[0][i].first; a; a=a->next)
{
if (IsNode0(a->head)) delta += a->sister->r_cap + GetMate(a)->sister->r_cap;
else delta -= a->r_cap + GetMate(a)->r_cap;
}
fprintf(fp, "1 %d %d\n", i+1, delta);
}
for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e))
{
GetTwicePairwiseTerm(e, i, j, E00, E01, E10, E11);
fprintf(fp, "2 %d %d %d\n", i+1, j+1, E00 + E11 - E01 - E10);
}
fclose(fp);
return true;
}
return false;
}
int main() {
int a, b;
scanf("%d %d", &a, &b);
printf("%d", Solve(b) - Solve(a - 1) );
}
bool CMOOSNavLSQEngine::Iterate(double dfTimeNow)
{
//house keeping
if(!IterateRequired(dfTimeNow))
{
return false;
}
//keep a count of how many times this has been called
if(!m_bGuessed)
{
GuessVehicleLocation();
m_bGuessed = true;;
}
m_dfTimeNow = dfTimeNow;
//take a copy of m_Xhat
m_XhatTmp = m_Xhat;
m_PhatTmp = m_Phat;
double m_dfCutOffTime = dfTimeNow-m_dfLSQWindow;
m_Observations.clear();
if(m_pStore->GetObservationsBetween(m_Observations,m_dfCutOffTime,dfTimeNow))
{
if(m_Observations.empty())
return false;
m_dfLastSolveAttempt = dfTimeNow;
//here we add in the fixed observations....
m_Observations.insert( m_Observations.begin(),
m_FixedObservations.begin(),
m_FixedObservations.end());
//limit the number of certain kinds of observations
LimitObservations(CMOOSObservation::DEPTH,1);
LimitObservations(CMOOSObservation::YAW,1);
LimitObservations(CMOOSObservation::X,1);
LimitObservations(CMOOSObservation::Y,1);
//here is out chance to look at consistency of data..
//PreFilterData();
//TraceObservationSet();
m_Logger.Comment("LSQ Iterate Attempted",dfTimeNow);
for(int nIteration = 0;nIteration<m_nLSQIterations;nIteration++)
{
if(MakeObsMatrices())
{
//we've just figured out innovations
//but don't have innov std - we record
//this data in each observation
RecordObsStatistics(&m_Innov,NULL);
switch(Solve())
{
case LSQ_NO_SOLUTION:
//failed update...
// MOOSTrace("No Solution!\n");
//TraceObservationSet();
m_Xhat = m_XhatTmp;
m_Phat = m_PhatTmp;
m_bTryToSolve = false;
m_Logger.Comment("No Solution",dfTimeNow);
return false;
case LSQ_BIG_CHANGE:
m_bTryToSolve = false;
m_Logger.Comment("Big Change In Solution - solution abandoned",dfTimeNow);
return false;
case LSQ_IN_PROGRESS:
if(nIteration==m_nLSQIterations)
{
m_Xhat = m_XhatTmp;
m_Phat = m_PhatTmp;
m_bTryToSolve = false;
m_Logger.Comment("No Convergence",dfTimeNow);
return false;
}
break;
case LSQ_SOLVED:
if(!DoWStatistic())
{
//we will have removed the observation
//that is not fitting...try to iterate again..
nIteration = 0;
break;
}
else
{
return OnSolved();
}
}
}
}
}
return false;
}
int CKinematics::InvertTransformCADtoActuators(double *Acts, double *xr, double *yr, double *zr, double *ar, double *br, double *cr)
{
double Tol=1e-6;
double d=0.1; // should be linear over this range
double x=0,y=0,z=0,a=0,b=0,c=0; // initial guess at answer
double Acts0[MAX_ACTUATORS],ActsX[MAX_ACTUATORS],ActsY[MAX_ACTUATORS];
double ActsZ[MAX_ACTUATORS],ActsA[MAX_ACTUATORS],ActsB[MAX_ACTUATORS],ActsC[MAX_ACTUATORS];
double A[3*4];
double ex,ey,ez,ea,eb,ec;
for (int i=0; i<100; i++)
{
// measure sensitivity
//
// assume majority is simply x -> Act0, y -> Act1, etc..
TransformCADtoActuators(x, y, z, a, b, c, Acts0);
TransformCADtoActuators(x+d,y, z, a, b, c, ActsX);
TransformCADtoActuators(x, y+d,z, a, b, c, ActsY);
TransformCADtoActuators(x, y, z+d,a, b, c, ActsZ);
TransformCADtoActuators(x, y, z ,a+d,b, c, ActsA);
TransformCADtoActuators(x, y, z ,a, b+d,c, ActsB);
TransformCADtoActuators(x, y, z ,a, b, c+d,ActsC);
// | x | | RX0 RY0 RZ0 | | R0 |
// | y | | RX1 RY1 RZ1 | = | R1 |
// | z | | RX2 RY2 RZ2 | | R2 |
A[0*4+0] = (ActsX[0]-Acts0[0])/d; // changes due to x
A[1*4+0] = (ActsX[1]-Acts0[1])/d;
A[2*4+0] = (ActsX[2]-Acts0[2])/d;
A[0*4+1] = (ActsY[0]-Acts0[0])/d; // changes due to y
A[1*4+1] = (ActsY[1]-Acts0[1])/d;
A[2*4+1] = (ActsY[2]-Acts0[2])/d;
A[0*4+2] = (ActsZ[0]-Acts0[0])/d; // changes due to z
A[1*4+2] = (ActsZ[1]-Acts0[1])/d;
A[2*4+2] = (ActsZ[2]-Acts0[2])/d;
A[0*4+3] = Acts[0]-Acts0[0]; // desired changes
A[1*4+3] = Acts[1]-Acts0[1];
A[2*4+3] = Acts[2]-Acts0[2];
Solve(A,3); // solve simultaneous eqs
// corrections in CAD space
ex = A[0*4+3];
ey = A[1*4+3];
ez = A[2*4+3];
ea = d*(Acts[3] - Acts0[3])/(ActsA[3] - Acts0[3]);
eb = d*(Acts[4] - Acts0[4])/(ActsB[4] - Acts0[4]);
ec = d*(Acts[5] - Acts0[5])/(ActsC[5] - Acts0[5]);
// Done if all within Tolerance
if (fabs(ex) < Tol &&
fabs(ey) < Tol &&
fabs(ez) < Tol &&
fabs(ea) < Tol &&
fabs(eb) < Tol &&
fabs(ec) < Tol)
{
*xr = x;
*yr = y;
*zr = z;
*ar = a;
*br = b;
*cr = c;
return 0;
}
// make a correction
x += ex;
y += ey;
z += ez;
a += ea;
b += eb;
c += ec;
}
// it never converges, return whatever we have
*xr = x;
*yr = y;
*zr = z;
*ar = a;
*br = b;
*cr = c;
return 1;
}
void cSetEqFormelles::SolveResetUpdate(double ExpectResidu,bool *OK)
{
Solve(ExpectResidu,OK);
ResetUpdate(1.0);
}
void lidarBoostEngine::build_superresolution(short coeff)
{
std::cout<< "Num of clouds : " << Y.size() << std::endl;
// std::cout << Y[0] << std::endl;
beta = coeff;
std::vector < MatrixXd > optflow = lk_optical_flow( Y[2], Y[4], 10 );
MatrixXd D( beta*n, beta*m ); //, X( beta*n, beta*m );
// SparseMatrix<double> W( beta*n, beta*m ), T( beta*n, beta*m );
D = apply_optical_flow(Y[2], optflow);
T = check_unreliable_samples(intensityMap[2], 0.0001);
MatrixXd up_D = nearest_neigh_upsampling(D);
//// Display and Debug
cv::Mat M(n, m, CV_32FC1);
// MatrixXd diff1(n, m);
// diff1 = MatrixXd::Ones(n, m) - Y[0];
cv::eigen2cv(Y[2], M);
cv::Mat M1(n, m, CV_32FC1);
cv::eigen2cv(Y[4], M1);
// MatrixXd diff(beta*n, beta*m);
// diff = MatrixXd::Ones(beta*n, beta*m) - up_D;
cv::Mat M2(beta*n, beta*m, CV_32FC1);
cv::eigen2cv(up_D, M2);
cv::namedWindow("check", cv::WINDOW_AUTOSIZE );
cv::imshow("check", M);
cv::namedWindow("check1", cv::WINDOW_AUTOSIZE );
cv::imshow("check1", M1);
cv::namedWindow("check2", cv::WINDOW_AUTOSIZE );
cv::imshow("check2", M2);
//// Solve example equation with eigen
// Eigen::VectorXd x(2);
// x(0) = 10.0;
// x(1) = 25.0;
// std::cout << "x: " << x << std::endl;
// my_functor functor;
// Eigen::NumericalDiff<my_functor> numDiff(functor);
// Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,double> lm(numDiff);
// lm.parameters.maxfev = 2000;
// lm.parameters.xtol = 1.0e-10;
// std::cout << lm.parameters.maxfev << std::endl;
// int ret = lm.minimize(x);
// std::cout << lm.iter << std::endl;
// std::cout << ret << std::endl;
// std::cout << "x that minimizes the function: " << x << std::endl;
////// Try to solve lidarboost with Eigen
// my_functor functor;
// Eigen::NumericalDiff<my_functor> numDiff(functor);
// Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,double> lm(numDiff);
// lm.parameters.maxfev = 2000;
// lm.parameters.xtol = 1.0e-10;
// std::cout << lm.parameters.maxfev << std::endl;
// VectorXd val(2);
// for(int i = 0; i < X.rows(); i++)
// {
// for(int j = 0; j < X.cols(); j++)
// {
// val = X(i, j);
// int ret = lm.minimize(val);
// }
// }
// std::cout << lm.iter << std::endl;
// std::cout << ret << std::endl;
// std::cout << "x that minimizes the function: " << X << std::endl;
//// Solve example using ceres
// The variable to solve for with its initial value.
// double initial_x = 5.0;
// double x = initial_x;
MatrixXd X(beta*n, beta*m);// init_X(beta*n, beta*m);
// X = MatrixXd::Zero(beta*n,beta*m);
X = up_D;
// MatrixXd init_X( beta*n, beta*m );
// init_X = X;
// int M[2][2], M2[2][2];
// M[0][0] = 5;
// M[1][0] = 10;
// M[0][1] = 20;
// M[1][1] = 30;
// M2 = *M;
// Build the problem.
Problem problem;
// Set up the only cost function (also known as residual). This uses
// auto-differentiation to obtain the derivative (jacobian).
double val, w, t, d;
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = false;
Solver::Summary summary;
for(int i = 0; i < X.rows(); i++)
{
for(int j = 0; j < X.cols(); j++)
{
val = X(i, j);
w = W(i, j);
t = T(i, j);
d = up_D(i, j);
std::cout << "i = " << i << "; j = " << j << std::endl;
std::cout << "w = " << w << "; t = " << t << "; d = " << d << std::endl;
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor(w, t, d));
problem.AddResidualBlock(cost_function, NULL, &val);
// Run the solver
Solve(options, &problem, &summary);
X(i, j) = val;
}
}
std::cout << summary.BriefReport() << "\n";
// std::cout << "x : " << init_X
// << " -> " << X << "\n";
cv::Mat M3(beta*n, beta*m, CV_32FC1);
cv::eigen2cv(X, M3);
cv::namedWindow("check3", cv::WINDOW_AUTOSIZE );
cv::imshow("check3", M3);
}
void testReprojectionError_plain(){
// initialize random number generator
//srand((unsigned int) time(0)); // disabled: make unit tests deterministic...
// Build the problem.
::ceres::Problem problem;
// set up a random geometry
// pose
std::cout<<"set up a random geometry... "<<std::flush;
okvis::kinematics::Transformation T_WS; // world to sensor
T_WS.setRandom(10.0,M_PI);
okvis::kinematics::Transformation T_WS_disturb;
T_WS_disturb.setRandom(1,0.01);
okvis::kinematics::Transformation T_WS_init=T_WS*T_WS_disturb; // world to sensor
// extrinsic parameter
okvis::kinematics::Transformation T_SC; // sensor to camera
T_SC.setRandom(0.2,M_PI);
// 设置 姿态和外部参数
okvis::ceres::PoseParameterBlock poseParameterBlock(T_WS_init,1,okvis::Time(0));
okvis::ceres::PoseParameterBlock extrinsicsParameterBlock(T_SC,2,okvis::Time(0));
problem.AddParameterBlock(poseParameterBlock.parameters(),okvis::ceres::PoseParameterBlock::Dimension);
problem.AddParameterBlock(extrinsicsParameterBlock.parameters(),okvis::ceres::PoseParameterBlock::Dimension);
problem.SetParameterBlockVariable(poseParameterBlock.parameters()); // optimize this...
problem.SetParameterBlockConstant(extrinsicsParameterBlock.parameters()); // do not optimize this...
std::cout<<" [ OK ] "<<std::endl;
// set up a random camera geometry
std::cout << "set up a random camera geometry... " << std::flush;
typedef okvis::cameras::PinholeCamera<okvis::cameras::EquidistantDistortion> DistortedPinholeCameraGeometry;
std::shared_ptr<const DistortedPinholeCameraGeometry> cameraGeometry =
std::static_pointer_cast<const DistortedPinholeCameraGeometry>(DistortedPinholeCameraGeometry::createTestObject());
std::cout << " [ OK ] " << std::endl;
// let's use our own local quaternion perturbation
std::cout<<"setting local parameterization for pose... "<<std::flush;
::ceres::LocalParameterization* poseLocalParameterization = new okvis::ceres::PoseLocalParameterization;
problem.SetParameterization(poseParameterBlock.parameters(),poseLocalParameterization);
problem.SetParameterization(extrinsicsParameterBlock.parameters(),poseLocalParameterization);
std::cout<<" [ OK ] "<<std::endl;
// and the parameterization for points:
::ceres::LocalParameterization* homogeneousPointLocalParameterization =
new okvis::ceres::HomogeneousPointLocalParameterization;
// get some random points and build error terms
const size_t N=100;
std::cout<<"create N="<<N<<" visible points and add respective reprojection error terms... "<<std::flush;
for (size_t i=1; i<100; ++i){
Eigen::Vector4d point = cameraGeometry->createRandomVisibleHomogeneousPoint(double(i%10)*3+2.0);
okvis::ceres::HomogeneousPointParameterBlock* homogeneousPointParameterBlock_ptr =
new okvis::ceres::HomogeneousPointParameterBlock(T_WS*T_SC*point,i+2);
problem.AddParameterBlock(homogeneousPointParameterBlock_ptr->parameters(),okvis::ceres::HomogeneousPointParameterBlock::Dimension);
problem.SetParameterBlockConstant(homogeneousPointParameterBlock_ptr->parameters());
// get a randomized projection
// 投影
Eigen::Vector2d kp;
cameraGeometry->projectHomogeneous(point,&kp);
kp += Eigen::Vector2d::Random();
// Set up the only cost function (also known as residual).
// 构造 cost function
Eigen::Matrix2d information=Eigen::Matrix2d::Identity();
::ceres::CostFunction* cost_function = new okvis::ceres::ReprojectionError<DistortedPinholeCameraGeometry>(
cameraGeometry,1, kp,information);
problem.AddResidualBlock(cost_function, NULL, poseParameterBlock.parameters(), homogeneousPointParameterBlock_ptr->parameters(), extrinsicsParameterBlock.parameters());
// set the parameterization
problem.SetParameterization(homogeneousPointParameterBlock_ptr->parameters(),homogeneousPointLocalParameterization);
}
std::cout<<" [ OK ] "<<std::endl;
// Run the solver!
std::cout<<"run the solver... "<<std::endl;
::ceres::Solver::Options options;
//options.check_gradients=true;
//options.numeric_derivative_relative_step_size = 1e-6;
//options.gradient_check_relative_precision=1e-2;
options.minimizer_progress_to_stdout = false;
::FLAGS_stderrthreshold=google::WARNING; // enable console warnings (Jacobian verification)
::ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
// verify there are no errors in the Jacobians
OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error);
// print some infos about the optimization
//std::cout << summary.BriefReport() << "\n";
std::cout << "initial T_WS : " << T_WS_init.T() << "\n"
<< "optimized T_WS : " << poseParameterBlock.estimate().T() << "\n"
<< "correct T_WS : " << T_WS.T() << "\n";
// make sure it converged
OKVIS_ASSERT_TRUE(Exception,2*(T_WS.q()*poseParameterBlock.estimate().q().inverse()).vec().norm()<1e-2,"quaternions not close enough");
OKVIS_ASSERT_TRUE(Exception,(T_WS.r()-poseParameterBlock.estimate().r()).norm()<1e-1,"translation not close enough");
}
main(int argc, char *argv[])
{
int myproc, numprocs, j, i;
MPI_Comm comm;
gridT *grid;
physT *phys;
propT *prop;
sediT *sedi;
spropT *sprop;
waveT *wave;
wpropT *wprop;
StartMpi(&argc,&argv,&comm,&myproc,&numprocs);
ParseFlags(argc,argv,myproc);
if(GRID)
GetGrid(&grid,myproc,numprocs,comm);
else
ReadGrid(&grid,myproc,numprocs,comm);
if(SOLVE) {
ReadProperties(&prop,myproc);
InitializeVerticalGrid(&grid);
AllocatePhysicalVariables(grid,&phys,prop);
// if(prop->wave){
InitializeWaveProperties(&wprop, prop, myproc);
AllocateWaveVariables(grid, &wave, prop, wprop);
InitializeWaveVariables(grid, wave, prop, wprop, myproc, comm);
// }
if(prop->sedi){
InitializeSediProperties(prop, &sprop, myproc);
AllocateSediVariables(grid, &sedi, sprop, prop);
InitializeSediVariables(grid, sedi, prop, sprop, myproc, comm);
}
AllocateTransferArrays(&grid,myproc,numprocs,comm);
OpenFiles(prop,myproc);
if(RESTART){
ReadPhysicalVariables(grid,phys,prop,sedi,sprop,wave,wprop,myproc,comm);
}
else{
InitializePhysicalVariables(grid,phys,sedi,prop,sprop);
}
InputTides(grid, phys, prop, myproc);
SetDragCoefficients(grid,phys,prop);
if (prop->wave)
if (wprop->wind_forcing){
ObtainKrigingCoef(grid, wave, wprop, myproc, numprocs);
for (i = 0; i< wprop->nstation; i++)
InputWind(i, prop, wave,wprop,myproc,numprocs);
}
Solve(grid,phys,prop,sprop,sedi,wave,wprop,myproc,numprocs,comm);
// FreePhysicalVariables(grid,phys,prop);
// FreeTransferArrays(grid,myproc,numprocs,comm);
}
EndMpi(&comm);
}
/* init board, route traces*/
void PCB_EDIT_FRAME::Autoroute( wxDC* DC, int mode )
{
int start, stop;
MODULE* Module = NULL;
D_PAD* Pad = NULL;
int autoroute_net_code = -1;
wxString msg;
if( GetBoard()->GetCopperLayerCount() > 1 )
{
g_Route_Layer_TOP = GetScreen()->m_Route_Layer_TOP;
g_Route_Layer_BOTTOM = GetScreen()->m_Route_Layer_BOTTOM;
}
else
{
g_Route_Layer_TOP = g_Route_Layer_BOTTOM = B_Cu;
}
switch( mode )
{
case ROUTE_NET:
if( GetScreen()->GetCurItem() )
{
switch( GetScreen()->GetCurItem()->Type() )
{
case PCB_PAD_T:
Pad = (D_PAD*) GetScreen()->GetCurItem();
autoroute_net_code = Pad->GetNetCode();
break;
default:
break;
}
}
if( autoroute_net_code <= 0 )
{
wxMessageBox( _( "Net not selected" ) ); return;
}
break;
case ROUTE_MODULE:
Module = (MODULE*) GetScreen()->GetCurItem();
if( (Module == NULL) || (Module->Type() != PCB_MODULE_T) )
{
wxMessageBox( _( "Footprint not selected" ) );
return;
}
break;
case ROUTE_PAD:
Pad = (D_PAD*) GetScreen()->GetCurItem();
if( (Pad == NULL) || (Pad->Type() != PCB_PAD_T) )
{
wxMessageBox( _( "Pad not selected" ) );
return;
}
break;
}
if( (GetBoard()->m_Status_Pcb & LISTE_RATSNEST_ITEM_OK ) == 0 )
Compile_Ratsnest( DC, true );
/* Set the flag on the ratsnest to CH_ROUTE_REQ. */
for( unsigned ii = 0; ii < GetBoard()->GetRatsnestsCount(); ii++ )
{
RATSNEST_ITEM* ptmp = &GetBoard()->m_FullRatsnest[ii];
ptmp->m_Status &= ~CH_ROUTE_REQ;
switch( mode )
{
case ROUTE_ALL:
ptmp->m_Status |= CH_ROUTE_REQ;
break;
case ROUTE_NET:
if( autoroute_net_code == ptmp->GetNet() )
ptmp->m_Status |= CH_ROUTE_REQ;
break;
case ROUTE_MODULE:
{
D_PAD* pt_pad = (D_PAD*) Module->Pads();
for( ; pt_pad != NULL; pt_pad = pt_pad->Next() )
{
if( ptmp->m_PadStart == pt_pad )
ptmp->m_Status |= CH_ROUTE_REQ;
if( ptmp->m_PadEnd == pt_pad )
ptmp->m_Status |= CH_ROUTE_REQ;
}
break;
}
case ROUTE_PAD:
if( ( ptmp->m_PadStart == Pad ) || ( ptmp->m_PadEnd == Pad ) )
ptmp->m_Status |= CH_ROUTE_REQ;
break;
}
}
start = time( NULL );
/* Calculation of no fixed routing to 5 mils and more. */
RoutingMatrix.m_GridRouting = (int)GetScreen()->GetGridSize().x;
if( RoutingMatrix.m_GridRouting < (5*IU_PER_MILS) )
RoutingMatrix.m_GridRouting = 5*IU_PER_MILS;
/* Calculated ncol and nrow, matrix size for routing. */
RoutingMatrix.ComputeMatrixSize( GetBoard() );
m_messagePanel->EraseMsgBox();
/* Map the board */
RoutingMatrix.m_RoutingLayersCount = 1;
if( g_Route_Layer_TOP != g_Route_Layer_BOTTOM )
RoutingMatrix.m_RoutingLayersCount = 2;
if( RoutingMatrix.InitRoutingMatrix() < 0 )
{
wxMessageBox( _( "No memory for autorouting" ) );
RoutingMatrix.UnInitRoutingMatrix(); /* Free memory. */
return;
}
SetStatusText( _( "Place Cells" ) );
PlaceCells( GetBoard(), -1, FORCE_PADS );
/* Construction of the track list for router. */
RoutingMatrix.m_RouteCount = Build_Work( GetBoard() );
// DisplayRoutingMatrix( m_canvas, DC );
Solve( DC, RoutingMatrix.m_RoutingLayersCount );
/* Free memory. */
FreeQueue();
InitWork(); /* Free memory for the list of router connections. */
RoutingMatrix.UnInitRoutingMatrix();
stop = time( NULL ) - start;
msg.Printf( wxT( "time = %d second%s" ), stop, ( stop == 1 ) ? wxT( "" ) : wxT( "s" ) );
SetStatusText( msg );
}
void MtxLP::optimise(stomap* targ, bool max, bool presolve){
setMixedObjective(targ);
setObjDir(max);
Solve(presolve);
}
void solveSudoku(vector<vector<char>>& board)
{
Solve(board);
return;
}
int main(int argc, char *argv[]) {
int q = 0;
int n = 1;
int max = 10;
double t1,t2;
int max_matrix_size = 10;
double *A;
double *b;
double *A1;
FILE* f;
int ret = 0;
opterr = 0;
while ((ret = getopt( argc, argv, "f::g:N:")) != -1) {
switch (ret) {
case 'N':
max = atoi(optarg);
printf("Max output size: %d\n", max);
break;
case 'f':
q = 1;
if (optarg != NULL) {
printf ("Input from file %s\n", optarg);
if ((f = fopen (optarg, "r")) == NULL) {
perror ("Can't open file.\n");
return -1;
}
} else {
printf ("Input from file input.txt\n");
if ((f = fopen ("input.txt", "r")) == NULL) {
perror ("Can't open file.\n");
return -1;
}
}
break;
case 'g':
printf("Input from function in file input_function.cpp\n");
q = 2;
n = atoi(optarg);
printf("Size of matrix: %d\n", n);
break;
case '?':
printf("Unknown option -%c\n", optopt);
printf("Supported options:\n");
printf("-f -- Input matrix from the argument file, or, if there is no argument, input matrix from the file input.txt;\n");
printf("-g -- Input matrix from the function. Argument of this option means size of matrix and it is required;\n");
printf("-N -- Maximum size of the solution, displayed on the screen. Argument is required.\n");
return -1;
}
}
if (q == 1) {
if(fscanf (f, "%d", &n) == 0) {
printf ("Problem with size of matrix.\n");
return -1;
}
A = new double [n*(n+1)];
b = new double [n];
A1 = new double [n*n];
if (A == NULL || b == NULL || A1 == NULL) {
printf ("Problem with memory allocation.\n");
return -1;
}
if(file_input(f, n, A, A1, b) != 0) {
return -1;
}
fclose(f);
} else if (q == 2) {
A = new double [n*(n+1)];
b = new double [n];
if (A == NULL || b == NULL) {
printf ("Problem with memory allocation.\n");
return -1;
}
if (func_input(A, b, n) != 0) {
return -1;
}
} else {
printf("It's recommended to run the program with boot options\n");
printf("Supported options:\n");
printf("-f -- Input matrix from the argument file, or, if there is no argument, input matrix from the file input.txt;\n");
printf("-g -- Input matrix from the function. Argument of this option means size of the matrix and it's required;\n");
printf("-N -- Maximum size of the solution, displayed on the screen. Argument is required.\n");
printf ("\n\n");
printf("Choose, where to take the matrix from:\n");
printf("1 -- from file input.txt\n");
printf("2 -- from input function in file input_function.cpp\n");
scanf("%d", &q);
if (q == 1) {
f = fopen("input.txt","r");
if(f == NULL) {
printf("Can't open input file.\n");
return -1;
}
if(fscanf (f, "%d", &n) == 0) {
printf("Problem with size of matrix.\n");
return -1;
}
A = new double [n*(n+1)/2];
b = new double [n];
A1 = new double [n*n];
if (A == NULL || b == NULL || A1 == NULL) {
printf ("Problem with memory allocation.\n");
return -1;
}
if(file_input(f, n, A,A1, b) != 0) {
return -1;
}
fclose(f);
} else if (q == 2) {
printf("Size of matrix: ");
scanf("%d", &n);
A = new double [n*(n+1)/2];
b = new double [n];
if (A == NULL || b == NULL) {
printf ("Problem with memory allocation.\n");
return -1;
}
if (func_input(A, b, n) != 0) {
return -1;
}
} else {
printf("Incorrect input way.\n");
return -1;
}
}
FILE *g;
g = fopen("output.txt", "wr");
if(g == NULL) {
printf("Can't open output file.\n");
return -1;
}
PrintMatrix(n,A,b,max_matrix_size);
double *Y = new double [n];
t1 = get_time();
Solve(A,b,n,Y);
t2 = get_time();
file_output(g,n,max,Y);
printf("Time = %f\n", t2-t1);
if (q == 1) {
printf("Residual norm = %e\n", residual_norm1(n,A1,b,Y));
delete []A1;
} else if(q == 2) {
printf("Residual norm = %e\n", residual_norm(n,b,Y));
printf("Error rate = %e\n", error_rate(n,Y));
}
delete []A;
delete []b;
delete []Y;
fclose(g);
return 0;
}
bool BoatAnalysisDlg::UnitLoop()
{
QString str;
int n, nrhs;
if (m_ControlMax<m_ControlMin) m_ControlDelta = -fabs(m_ControlDelta);
nrhs = (int)fabs((m_ControlMax-m_ControlMin)*1.0001/m_ControlDelta) + 1;
if(!m_bSequence) nrhs = 1;
else if(nrhs>=100)
{
QMessageBox::warning(this, tr("Warning"),tr("The number of points to be calculated will be limited to 100"));
nrhs = 100;
}
//ESTIMATED UNIT TIMES FOR OPERATIONS
double TotalTime = 10.0*(double)m_MatSize/400. //BuildInfluenceMatrix : 10 x MatSize/400
+ 10. //CreateRHS : 10
+ 30.*(double)m_MatSize/400. //SolveUnitRHS : 30 x MatSize/400
+ 1./400. * (double)m_pBoat->m_poaSail.size() //ComputeFarField : 1 x MatSize/400x nsails
+ 1. ; //ComputeOnBodyCp : 1
TotalTime *= (double)nrhs;
m_pctrlProgress->setMinimum(0);
m_pctrlProgress->setMaximum((int)TotalTime);
m_Progress = 0.0;
qApp->processEvents();
str = QString(tr(" Solving the problem... ")+"\n");
AddString(str);
for (n=0; n<nrhs; n++)
{
m_Ctrl = m_ControlMin +(double)n * m_ControlDelta;
str = QString(" \n "+tr("Processing parameter= %1")+"\n").arg(m_Ctrl,8,'f',3);
AddString(str);
SetAngles(m_pBoatPolar, m_Ctrl, false);
if (m_bCancel) return true;
BuildInfluenceMatrix();
if (m_bCancel) return true;
CreateRHS(m_RHS);
if (m_bCancel) return true;
CreateSourceStrength();
if (m_bCancel) return true;
//compute wake contribution
// CreateWakeContribution();
if (m_bCancel) return true;
//add wake contribution to matrix and RHS
// for(int p=0; p<m_MatSize; p++)
// {
// m_uRHS[p]+= m_uWake[p];
// m_vRHS[p]+= m_wWake[p];
/* for(int pp=0; pp<m_MatSize; pp++)
{
s_aij[p*m_MatSize+pp] += s_aijWake[p*m_MatSize+pp];
}*/
// }
if (m_bCancel) return true;
if (!Solve())
{
m_bWarning = true;
return true;
}
if (m_bCancel) return true;
ComputeFarField();
if (m_bCancel) return true;
ComputeOnBody();
if (m_bCancel) return true;
ComputeBoat();
if (m_bCancel) return true;
}
return true;
}
int main(int argc, char *argv[]){
int mode=0;
int n = 1;
int t = 3;
int max = 10;
double *A;
double *b;
double t1;
FILE* input;
int ret = 0;
opterr = 0;
while ((ret = getopt( argc, argv, "f::g:N:t:")) != -1) {
switch (ret) {
case 'N':
if (optarg == NULL) return -1;
max = atoi(optarg);
printf("максимальный выходной размер: %d\n", max);
break;
case 't':
if (optarg == NULL) return -1;
t = atoi(optarg);
break;
case 'f':
mode = 1;
if (optarg != NULL){
printf ("ввод из файла %s\n", optarg);
if ((input = fopen (optarg, "r")) == NULL) {
perror ("невозможно открыть файл\n");
return -1;
}
} else {
printf ("ввод из файла input.txt\n");
if ((input = fopen ("input.txt", "r")) == NULL) {
perror ("невозможно открыть файл\n");
return -1;
}
}
break;
case 'g':
if (mode == 1) break;
if (optarg == NULL) return -1;
printf("ввод из функции\n");
mode = 2;
n = atoi(optarg);
printf("матрица размера: %d\n", n);
break;
case '?':
printf("неизвестная опция -%c\n", optopt);
printf("поддерживаемые опции:\n");
printf("f - ввод матрицы из файла, являющегося аргументом, или, если аргумента нет, ввод матрицы из файла input.txt;\n");
printf("g - ввод матрицы из функции. Аргумент функции (обязателен) является размером матрицы;\n");
printf("t - количество нитей;\n");
printf("N - максимальный выходной размер.\n");
return -1;
}
}
printf("Используется %d нитей.\n", t);
if (mode == 1){
if(fscanf (input, "%d", &n) == 0){
printf ("не получилось сосканировать размер матрицы из файла\n");
return -1;
}
A = new double [n*(n+1)];
if (A == NULL){
printf("не удалось выделить память под матрицу А\n");
return -1;
}
if (file_input(input, n, A) != 0){
return -1;
}
fclose(input);
} else if (mode == 2){
A = new double [n*(n+1)];
if (func_input(A, n) != 0){
return -1;
}
} else {
printf("Требуется запустить программу с какими-либо параметрами\n");
printf("Поддерживаемые параметры запуска:\n");
printf("f - ввод матрицы из файла, являющегося аргументом, или, если аргумента нет, ввод матрицы из файла input.txt;\n");
printf("g - ввод матрицы из функции. Аргумент функции (обязателен) является размером матрицы;\n");
printf("t - количество нитей;\n");
printf("N - опция, аргумент которой (обязателен) задает максимальный выходной размер.\n");
return -1;
}
pthread_t *threads = new pthread_t [t]; //инициализируем нити, выделяем под них память
if (threads == NULL){
printf("не удалось выделить память под массив нитей\n");
return -1;
}
b = new double [n];
if (b == NULL){
printf("не удалось выделить память под вектор b\n");
return -1;
}
for (int z=0;z<n;z++){
b[z] = -A[z*(n+1)+n];
}
int MAX_OUTPUT_SIZE = 10;
PrintMatrix(n, A, b, MAX_OUTPUT_SIZE);
delete []b;
printf("\n");
FILE *out;
out = fopen("output.txt", "wr");
if(out == NULL){
printf("не могу открыть выходной файл\n");
return -1;
}
double *x = new double [n]; //сюда пишем решение
if (x == NULL){
printf("не удалось выделить память под вектор x\n");
return -1;
}
double *E = new double [(n+1)*(n+1)]; //здесь будут храниться базисы
if (E == NULL){
printf("не удалось выделить память под матрицу Е\n");
return -1;
}
solve *args = new solve [t]; //массив структур для нитей
if (args == NULL){
printf("не удалось выделить память под аргументы нитей\n");
return -1;
}
double *v = new double [n+1]; //сюда запоминается вектор для master
if (v == NULL){
printf("не удалось выделить память под вектор v\n");
return -1;
}
printf("\nрешение системы...\n");
t1 = get_full_time(); //get_full_time для счета астрономического времени
Solve(n,A,x,t,threads,E,args,v);
t1 = get_full_time() - t1;
Nevyaska * arg = new Nevyaska [t];
if (args == NULL){
printf("не удалось выделить память под структуру значений для невязки\n");
return -1;
}
printf("\n");
file_output(out,n,max,x);
printf("время решения системы = %f\n", t1);
printf("\nподсчет невязки...\n");
printf("невязка = %e\n", nev(n,A,x,t,threads,arg));
if (mode == 2){
printf("норма погрешности = %e\n", error_rate(n,x));
}
delete []A;
delete []x;
delete []E;
delete []v;
delete []args;
delete []threads;
delete []arg;
fclose(out);
return 0;
}
// Solve the puzzle
// PRE: numberPegs is the number of pegs left, must be
// less then the boardSize-2 and greater than 0
// POST: returns 1 and a solved board (moves/board updated)
// on success, else returns 0 (and only moves is modified)
int Solve(int numberPegs) {
// struct to hold possible move offsets
struct PossibleMoves {
int midX,midY; // Offsets to jumped peg
int toX,toY; // Offsets to destination
};
// Moves to try for each position
static const PossibleMoves pm[6]= {
{1,0, 2,0},
{-1,0, -2,0},
{0,1, 0,2},
{0,-1, 0,-2},
{1,1, 2,2},
{-1,-1, -2,-2}
};
int x,y,z; // index variables for looping
int temp; // the current from peg position
// Width/height of the matrix (starting at one and ignoring border)
static int matrixSize=GetY(boardSize);
// base case
if(numberPegs==1) {
return 1;
}
// Loop through the y coordinates
for(y=1; y<matrixSize+1; y++) {
// Loop through x coordinates
for(x=1; x<y+1; x++) {
// Check that the from location is a peg
if(board[x][y]==1) {
// Set the from location
temp=boardSize-numberPegs-2;
moves[temp].fromX=x;
moves[temp].fromY=y;
// Loop through possible moves
for(z=0; z<6; z++) {
// Attempt a move if possible
if(board[x+pm[z].midX][y+pm[z].midY]==1
&& !board[x+pm[z].toX][y+pm[z].toY]) {
moves[temp].toX=x+pm[z].toX;
moves[temp].toY=y+pm[z].toY;
moves[temp].midX=x+pm[z].midX;
moves[temp].midY=y+pm[z].midY;
DoMove(numberPegs);
if(Solve(numberPegs-1)) {
return 1;
} else {
UnMove(numberPegs);
}
}
}
}
}
}
return 0;
}
bool QPSolver::ReadProblemAndSolve(const string& fileName, VectorXd& sol, SolutionMethod method)
{
string realFileName = fileName;
realFileName += "Lhs";
EigenSerializer::ReadMatrixFromFileDouble(realFileName, mLhs);
realFileName = fileName;
realFileName += "Rhs";
EigenSerializer::ReadVectorFromFileDouble(realFileName, mRhs);
realFileName = fileName;
realFileName += "Q";
EigenSerializer::ReadMatrixFromFileDouble(realFileName, mQ);
realFileName = fileName;
realFileName += "C";
EigenSerializer::ReadVectorFromFileDouble(realFileName, mc);
realFileName = fileName;
realFileName += "A";
EigenSerializer::ReadMatrixFromFileDouble(realFileName, mA);
realFileName = fileName;
realFileName += "Lbc";
EigenSerializer::ReadVectorFromFileDouble(realFileName, mlbc);
realFileName = fileName;
realFileName += "Ubc";
EigenSerializer::ReadVectorFromFileDouble(realFileName, mubc);
realFileName = fileName;
realFileName += "Blb";
EigenSerializer::ReadVectorFromFileInt(realFileName, mbConstraintLowerBounded);
realFileName = fileName;
realFileName += "Bub";
EigenSerializer::ReadVectorFromFileInt(realFileName, mbConstraintUpperBounded);
//int startConId = 0;
//int endConId = 48;
//mNumVar = mA.cols();
//mNumCon = mA.rows();
//mNumCon = endConId - startConId;
//double scale = 1;
//MatrixXd newA = scale * mA.block(startConId, 0, mNumCon, mNumVar);
//VectorXd newlbc = scale * mlbc.segment(startConId, mNumCon);
//VectorXd newubc = scale * mubc.segment(startConId, mNumCon);
//VectorXi newblb = mbConstraintLowerBounded.segment(startConId, mNumCon);
//VectorXi newbub = VectorXi::Zero(mNumCon);//mbConstraintUpperBounded.segment(startConId, mNumCon);
//mA = newA;
//mlbc = newlbc;
//mubc = newubc;
//mbConstraintLowerBounded = newblb;
//mbConstraintUpperBounded = newbub;
mlb = VectorXd::Zero(mNumVar);
mub = VectorXd::Zero(mNumVar);
mbLowerBounded = VectorXi::Zero(mNumVar);
mbUpperBounded = VectorXi::Zero(mNumVar);
return Solve(sol, method);
}
Im1D_REAL8 L2SysSurResol::V_GSSR_Solve(bool * aResOk)
{
return Solve(aResOk);
}
//main
int main(int argc, char** argv)
{
//user arguments.
unsigned int np = 6; //num of points
//initial guess
Eigen::Vector3d iguess_center(0.332,0.507,1.034);
// Eigen::Vector3d iguess_center(0.489543, 0.443138, 1.08142);
Eigen::Matrix3d iguess_rMat;
iguess_rMat << 0,1,0,
1,0,0,
0,0,-1;
Eigen::Quaterniond iguess_orientation(iguess_rMat); //initial guess orientation of base wrt the camera
//params to be optimized
Eigen::Vector3d optimized_center(iguess_center);
Eigen::Quaterniond optimized_orientation(iguess_orientation);
//required inits for ceres
google::InitGoogleLogging(argv[0]);
//************** SET MEASUREMENTS *************************
//measured points in the image
Eigen::MatrixXd points_image(2,np);
points_image << 644, 617, 680, 785, 737, 908,
36, 274, 152, 202, 327, 160;
// points_image << 1095, 986, 1130, 873, 717, 679, 631, 681, 725, 680,
// 414, 517, 561, 482, 424, 465, 492, 513, 543, 488;
// points_image << 873, 717, 679, 631, 681, 725, 680,
// 482, 424, 465, 492, 513, 543, 488;
//measured points wrt the robot base
Eigen::MatrixXd points_base(3,np);
points_base << -0.600, -0.547, -0.573, -0.562, -0.534, -0.573,
-0.484, -0.492, -0.475, -0.455, -0.46, -0.427,
0.658, 0.66, 0.67, 0.669, 0.666, 0.657;
// points_base << -0.504, -0.457, -0.439, -0.465, -0.493, -0.470, -0.457, -0.446, -0.430, -0.460,
// -0.282, -0.33, -0.267, -0.358, -0.427, -0.446, -0.471, -0.444, -0.42, -0.44,
// 0.334, 0.32, 0.335, 0.211, 0.1789, 0.188, 0.179, 0.188, 0.179, 0.181;
// points_base << -0.465, -0.493, -0.470, -0.457, -0.446, -0.430, -0.460,
// -0.358, -0.427, -0.446, -0.471, -0.444, -0.42, -0.44,
// 0.211, 0.1789, 0.188, 0.179, 0.188, 0.179, 0.181;
Eigen::Matrix3d matrixK;
matrixK << 1614.930815, 0, 651.174560,
0, 1616.359896, 360.512415,
0, 0, 1;
//***************************************************************
//*************** CERES PROBLEM *********************************
// Declare the ceres problem.
ceres::Problem problem;
//Add constraints to the problem
for (unsigned int ii=0; ii<np; ii++)
{
//add a residual block for each measurement
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<pointConstraint,2,3,4>(new pointConstraint(ii, points_image.block<2,1>(0,ii), points_base.block<3,1>(0,ii), matrixK ) ),
//new ceres::CauchyLoss(0.5),
nullptr,
optimized_center.data(),
optimized_orientation.coeffs().data() );
}
// Apply the parameterization over the 4 quaternion parameters
ceres::LocalParameterization *eigen_quaternion_parameterization = new ceres::EigenQuaternionParameterization;
problem.SetParameterization(optimized_orientation.coeffs().data(), eigen_quaternion_parameterization);
//solve: estimate transform of base wrt camera
ceres::Solver::Options options;
options.minimizer_type = ceres::TRUST_REGION; //ceres::LINE_SEARCH
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 100;
options.function_tolerance = 1e-9;
options.gradient_tolerance = 1e-13;
options.parameter_tolerance = 1e-11;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
Solve(options, &problem, &summary);
//print results
std::cout << summary.FullReport() << std::endl;
std::cout << "iguess_center Base wrt camera : " << iguess_center.transpose() << std::endl;
std::cout << "iguess_orientation Base wrt camera: " << iguess_orientation.coeffs().transpose() << std::endl;
std::cout << "optimized_center Base wrt camera : " << optimized_center.transpose() << std::endl;
std::cout << "optimized_orientation Base wrt camera: " << optimized_orientation.coeffs().transpose() << std::endl;
//convert to homogeneous matrix
Eigen::Affine3d TB_C, TC_B;
TB_C.linear() = optimized_orientation.toRotationMatrix();
TB_C.translation() = optimized_center;
TC_B = TB_C.inverse();
Eigen::Vector3d transC_B;
transC_B << TC_B.matrix()(0,3),TC_B.matrix()(1,3),TC_B.matrix()(2,3);
Eigen::Quaterniond quatC_B(TC_B.linear());
std::cout << std::endl;
std::cout << "transC_B : " << transC_B.transpose() << std::endl;
std::cout << "quatC_B: " << quatC_B.coeffs().transpose() << std::endl;
//exit
return 0;
}
TEST_F(EmbeddedExplicitRungeKuttaNyströmIntegratorTest, Restart) {
AdaptiveStepSizeIntegrator<ODE> const& integrator =
EmbeddedExplicitRungeKuttaNyströmIntegrator<
methods::DormandالمكاوىPrince1986RKN434FM,
Length>();
Length const x_initial = 1 * Metre;
Speed const v_initial = 0 * Metre / Second;
Speed const v_amplitude = 1 * Metre / Second;
Time const period = 2 * π * Second;
AngularFrequency const ω = 1 * Radian / Second;
Instant const t_initial;
Time const duration = 10 * period;
Length const length_tolerance = 1 * Milli(Metre);
Speed const speed_tolerance = 1 * Milli(Metre) / Second;
// The number of steps if no step limit is set.
std::int64_t const steps_forward = 132;
auto const step_size_callback = [](bool tolerable) {};
std::vector<ODE::SystemState> solution1;
{
ODE harmonic_oscillator;
harmonic_oscillator.compute_acceleration =
std::bind(ComputeHarmonicOscillatorAcceleration1D,
_1, _2, _3, /*evaluations=*/nullptr);
IntegrationProblem<ODE> problem;
problem.equation = harmonic_oscillator;
problem.initial_state = {{x_initial}, {v_initial}, t_initial};
auto const append_state = [&solution1](ODE::SystemState const& state) {
solution1.push_back(state);
};
AdaptiveStepSizeIntegrator<ODE>::Parameters const parameters(
/*first_time_step=*/duration,
/*safety_factor=*/0.9,
/*max_steps=*/std::numeric_limits<std::int64_t>::max(),
/*last_step_is_exact=*/false);
auto const tolerance_to_error_ratio =
std::bind(HarmonicOscillatorToleranceRatio,
_1, _2,
length_tolerance,
speed_tolerance,
step_size_callback);
auto const instance = integrator.NewInstance(problem,
append_state,
tolerance_to_error_ratio,
parameters);
auto outcome = instance->Solve(t_initial + duration);
EXPECT_EQ(termination_condition::Done, outcome.error());
// Check that the time step has been updated.
EXPECT_EQ(131, solution1.size());
EXPECT_THAT(
solution1[solution1.size() - 1].time.value -
solution1[solution1.size() - 2].time.value,
AlmostEquals(0.509'363'975'335'290'320 * Second, 0));
// Restart the integration.
outcome = instance->Solve(t_initial + 2.0 * duration);
EXPECT_EQ(termination_condition::Done, outcome.error());
// Check that the time step has been updated again.
EXPECT_EQ(261, solution1.size());
EXPECT_THAT(
solution1[solution1.size() - 1].time.value -
solution1[solution1.size() - 2].time.value,
AlmostEquals(0.506'410'259'195'249'068 * Second, 0));
}
// Do it again in one call to |Solve| and check associativity.
std::vector<ODE::SystemState> solution2;
{
ODE harmonic_oscillator;
harmonic_oscillator.compute_acceleration =
std::bind(ComputeHarmonicOscillatorAcceleration1D,
_1, _2, _3, /*evaluations=*/nullptr);
IntegrationProblem<ODE> problem;
problem.equation = harmonic_oscillator;
problem.initial_state = {{x_initial}, {v_initial}, t_initial};
auto const append_state = [&solution2](ODE::SystemState const& state) {
solution2.push_back(state);
};
AdaptiveStepSizeIntegrator<ODE>::Parameters const parameters(
/*first_time_step=*/duration,
/*safety_factor=*/0.9,
/*max_steps=*/std::numeric_limits<std::int64_t>::max(),
/*last_step_is_exact=*/false);
auto const tolerance_to_error_ratio =
std::bind(HarmonicOscillatorToleranceRatio,
_1, _2,
length_tolerance,
speed_tolerance,
step_size_callback);
auto const instance = integrator.NewInstance(problem,
append_state,
tolerance_to_error_ratio,
parameters);
auto outcome = instance->Solve(t_initial + 2.0 * duration);
EXPECT_EQ(termination_condition::Done, outcome.error());
}
EXPECT_THAT(solution2, ElementsAreArray(solution1));
}
bool CPolynomialSolver::Polyfit(int nRows, int nOrder, TDataPoint *pData)
{
int rows = nRows;
int order = nOrder;
m_nGlobalO = order;
double *base = new double[order * rows];
double *alpha = new double[order * order];
double *alpha2 = new double[order * order];
double *beta = new double[order];
int j = 0;
int i=0;
int k,k2,k3;
// calc base
for (i = 0; i < order; i ++)
{
for (j = 0; j < rows; j ++)
{
int k = i + j * order;
base[k] = i == 0 ? 1.0 : pData[j].fXVal * base[k - 1];
}
}
// calc alpha2
for (i = 0; i < order; i ++)
{
for (j = 0; j <= i; j ++)
{
double sum = 0.0;
for ( k = 0; k < rows; k ++)
{
k2 = i + k * order;
k3 = j + k * order;
sum += base[k2] * base[k3];
}
k2 = i + j * order;
alpha2[k2] = sum;
if (i != j)
{
k2 = j + i * order;
alpha2[k2] = sum;
}
}
}
// calc beta
for (j = 0; j < order; j ++)
{
double sum = 0;
for ( k = 0; k < rows; k ++)
{
k3 = j + k * order;
sum += pData[k].fYVal * base[k3];
}
beta[j] = sum;
}
// get alpha
for (j = 0; j < order * order; j ++)
alpha[j] = alpha2[j];
// solve for params
bool bRes = Solve(alpha, beta, order);
for (j = 0; j < order; j ++)
m_fC[j] = beta[j];
delete base;
delete beta;
delete alpha;
delete alpha2;
return bRes;
}
void EQS_D2D::Execute( PROJECT* project, int steadyFlow, int linearShape )
{
MODEL* model = project->M2D;
GRID* rg = project->M2D->region;
NODE** node = model->node;
ELEM** elem = model->elem;
int np = model->np;
int ne = model->ne;
int div_cg = 0;
double* B = NULL;
double* X = NULL;
model->Incinit();
// print information on actual iteration -----------------------------------------------
project->PrintTheCycle( 1 );
REPORT::rpt.PrintTime( 1 );
// set parameters according to time integration and relaxation -------------------------
double th = project->timeint.thetaTurb;
double dt = project->timeint.incTime.Getsec();
if( fabs(th) < 1.0e-10 && fabs(dt) < 1.0e-10 )
{
REPORT::rpt.Error( kParameterFault, "theta and timeInterval too small (EQS_D2D::execute - 1)" );
}
double thdt = 1.0 / dt / th;
double dt_KD;
double relaxDt_KD = project->timeint.relaxTimeTurb.Getsec();
int relaxMethod = project->relaxMethod;
if( steadyFlow )
{
if( relaxMethod >= 3 )
{
dt_KD = relaxDt_KD;
relaxThdt_KD = 1.0 / dt_KD;
}
else
{
dt_KD = dt;
relaxThdt_KD = 0.0;
}
for( int i=0; i<np; i++ )
{
node[i]->v.dDdt = 0.0;
}
}
else
{
if( relaxMethod >= 3 )
{
dt_KD = relaxDt_KD;
}
else
{
dt_KD = dt;
}
relaxThdt_KD = 1.0 / dt_KD / th;
// time prediction -------------------------------------------------------------------
for( int i=0; i<np; i++ )
{
VARS* v = &node[i]->v;
//VARS* vo = &(node[i]->vo);
//v->D = vo->D + vo->dDdt * dt;
//v->dDdt = (1.0 - 1.0/th)*vo->dDdt + thdt*(v->D - vo->D);
v->dDdt = 0.0;
}
}
// check KD-values for validity (>= 0) -------------------------------------------------
Validate( np, node, project );
// determine friction coefficients -----------------------------------------------------
model->DoFriction( project );
// initialize Reynolds stresses and eddy viscosity -------------------------------------
rg->Turbulence( project );
// set KD boundary conditions ----------------------------------------------------------
model->SetBoundKD( project );
// -------------------------------------------------------------------------------------
int conv = true;
for( int it=0; it<project->actualCycit; it++ )
{
conv = true;
// print information on actual iteration ---------------------------------------------
if( it > 0 )
{
project->PrintTheCycle( it+1 );
REPORT::rpt.PrintTime( 1 );
}
// initialize Reynolds stresses and eddy viscosity -----------------------------------
if( it > 0 && isFS(project->actualTurb, BCONSET::kVtIterat)
&& !isFS(project->actualTurb, BCONSET::kVtPrandtlKol) )
{
rg->Turbulence( project );
}
// set up equation numbers -----------------------------------------------------------
if( model->Getinit() != modelInit )
{
initStructure = true;
modelInit = model->Getinit();
project->fix[0] = BCON::kFixD;
project->elemKind = ELEM::kRegion;
SetEqno( model, 1, 1, 0, project->fix, project->elemKind );
if( B ) MEMORY::memo.Detach( B );
if( X ) MEMORY::memo.Detach( X );
B = (double*) MEMORY::memo.Array_eq( neq );
X = (double*) MEMORY::memo.Array_eq( neq );
}
// solve equations with frontal solving algorithm ------------------------------------
for( int i=0; i<neq; i++ ) X[i] = 0.0;
div_cg = Solve( model, neq, B, X, project );
// -----------------------------------------------------------------------------------
// statistics
REPORT::rpt.Message( 1, "\n\n%-25s%s\n\n %s\n\n",
" (EQS_D2D::Execute)","convergence parameters ...",
" variable node average maximum" );
// compute averaged changes and standard deviation of D ------------------------------
double stdevD = 0.0;
double aveAbsD = 0.0;
for( int i=0; i<np; i++ )
{
int eqno;
double dD = 0.0;
eqno = GetEqno( node[i], 0 );
if( eqno >= 0 ) dD = X[eqno];
aveAbsD += dD;
stdevD += dD*dD;
}
int nptot = 0;
for( int i=0; i<np; i++ )
{
NODE* nd = rg->Getnode(i);
if( !isFS(nd->flag, NODE::kInface_DN) ) nptot++;
}
//////////////////////////////////////////////////////////////////////////////////////
// MPI: broadcast statistic
# ifdef _MPI_
aveAbsD = project->subdom.Mpi_sum( aveAbsD );
stdevD = project->subdom.Mpi_sum( stdevD );
nptot = project->subdom.Mpi_sum( nptot );
# endif
//////////////////////////////////////////////////////////////////////////////////////
aveAbsD /= nptot;
stdevD = sqrt( stdevD / nptot );
double norm = stdevD;
double fractD = 2.0 * sqrt( stdevD );
// compute maximum changes of K and D limited to ~95% fractile -----------------------
int cntD = 0;
int noPerD = 0;
double avePerD = 0.0;
double maxPerD = 0.0;
int noAbsD = 0;
double maxAbsD = 0.0;
for( int i=0; i<np; i++ )
{
int eqno;
double dK = 0.0;
double dD = 0.0;
eqno = GetEqno( node[i], 0 );
if( eqno >= 0 )
{
dD = X[eqno];
if( fabs(dD) > project->maxDeltaKD && fabs(dD) > fractD )
{
X[eqno] = dD/fabs(dD) * fractD;
}
}
if( node[i]->v.D + dD > 0.0 )
{
if( fabs(dD) > fabs(maxAbsD) )
{
maxAbsD = dD;
noAbsD = node[i]->Getname();
}
if( node[i]->v.D > 0.0 )
{
double per = dD / node[i]->v.D;
avePerD += fabs(per);
cntD++;
if( fabs(per) > fabs(maxPerD) )
{
maxPerD = per;
noPerD = node[i]->Getname();
}
}
}
}
//////////////////////////////////////////////////////////////////////////////////////
// MPI: broadcast statistic
# ifdef _MPI_
cntD = project->subdom.Mpi_sum( cntD );
maxAbsD = project->subdom.Mpi_max( maxAbsD );
avePerD = project->subdom.Mpi_sum( avePerD );
maxPerD = project->subdom.Mpi_max( maxPerD );
# endif
//////////////////////////////////////////////////////////////////////////////////////
avePerD /= cntD;
REPORT::rpt.Message( 1, " %1c %5d %12.5le %12.5le %s\n",
'D', noAbsD+1, aveAbsD, maxAbsD, " (abs)" );
REPORT::rpt.Message( 1, " %1c %5d %12.5lf %12.5lf %s\n\n",
' ', noPerD+1, avePerD, maxPerD, " ( % )" );
if( fabs(maxAbsD) > project->convKD ) conv = false;
// determine relaxation parameter for NEWTON-RAPHSON ---------------------------------
double relax;
double maxKD = fabs(maxAbsD);
switch( relaxMethod )
{
default:
REPORT::rpt.Warning( kParameterFault,
"relaxation method %d not supported", relaxMethod );
case 0:
relax = 1.0;
REPORT::rpt.Message( 1, "\n%-25s%s %12.4le\n %s %12.4le\n\n",
" ", "relaxation: norm =", norm,
" ", " relax =", relax );
break;
case 2:
relax = project->maxDeltaKD / maxKD;
if( relax > 1.0 ) relax = 1.0;
REPORT::rpt.Message( 1, "\n%-25s%s %12.4le\n%-25s%s %12.4le\n\n",
" ", "relaxation: norm =", norm,
" ", " relax =", relax );
break;
case 3:
case 4:
REPORT::rpt.Message( 1, "\n%-25s%s %12.4le\n",
" ", "relaxed time: dt_KD =", dt_KD );
if( dt_KD < dt ) conv = false;
relax = project->maxDeltaKD / maxKD;
dt_KD *= relax;
if( dt_KD > dt ) dt_KD = dt;
if( dt_KD < relaxDt_KD ) dt_KD = relaxDt_KD;
if( steadyFlow ) relaxThdt_KD = 1.0 / dt_KD;
else relaxThdt_KD = 1.0 / dt_KD / th;
if( relax > 1.0 ) relax = 1.0;
REPORT::rpt.Message( 1, "\n%-25s%s %12.4le\n%-25s%s %12.4le\n\n",
" ", "relaxation: norm =", norm,
" ", " relax =", relax );
break;
}
// update ----------------------------------------------------------------------------
for( int i=0; i<np; i++ )
{
int n = GetEqno( node[i], 0 );
if( n >= 0 ) node[i]->v.D += relax * X[n];
}
// check for range of values ---------------------------------------------------------
double minD = 0.0;
double maxD = 0.0;
int first = true;
int jd = 0;
for( int i=0; i<np; i++ )
{
if( first )
{
first = false;
minD =
maxD = node[i]->v.D;
}
else
{
if( node[i]->v.D < minD ) minD = node[i]->v.D;
if( node[i]->v.D > maxD ) maxD = node[i]->v.D;
}
if( node[i]->v.D <= 0.0 )
{
// node[i]->v.D = project->minD;
if( GetEqno(node[i],1) >= 0 ) jd++;
}
}
// compute useful values for K and D where they are negative -------------------------
if( jd )
{
REPORT::rpt.Message( 1, "%-25s%d %s\n\n", " ", jd, "nodes with D out of range" );
Validate( np, node, project );
}
REPORT::rpt.Message( 1, "%-25sminimum of D: %le\n", " ", minD );
REPORT::rpt.Message( 1, "%-25smaximum of D: %le\n", " ", maxD );
// compute time derivatives ----------------------------------------------------------
rg->ReportCuPe( dt, project->vk );
if( !steadyFlow )
{
double iTheta;
iTheta = 1.0 - 1.0 / project->timeint.thetaTurb;
for( int i=0; i<np; i++ )
{
if( isFS(node[i]->flag, NODE::kDry)
|| isFS(node[i]->flag, NODE::kMarsh) )
{
node[i]->v.dDdt = 0.0;
}
else
{
double D, pD, pdDdt;
VARS *v, *vo;
v = &(node[i]->v);
vo = &(node[i]->vo);
D = v->D;
pD = vo->D;
pdDdt = vo->dDdt;
// compute derivatives at actual time step
node[i]->v.dDdt = iTheta*pdDdt + thdt*(D - pD);
}
}
}
if( conv ) break;
}
// finally: compute eddy viscosity from revised turbulence parameters K,D --------------
rg->Turbulence( project );
// -------------------------------------------------------------------------------------
MEMORY::memo.Detach( B );
MEMORY::memo.Detach( X );
if( !conv ) project->errLevel |= kErr_some_errors | kErr_no_conv_nr;
if( div_cg ) project->errLevel |= div_cg | kErr_no_conv_cg;
}
//-----------------------------------------------------------------------
//mesh square parameterize
//-----------------------------------------------------------------------
void OMPmodel::Param(const char *infile, const char *outfile, int solveType, int outType)
{
// read mesh from file
if (!OpenMesh::IO::read_mesh(mesh, infile))
{
std::cerr << "Error: Cannot read mesh from " << infile << std::endl;
}
// this vertex property stores the vertex id
OpenMesh::VPropHandleT<uint> vertexID;
mesh.add_property(vertexID);
uint i = 0;
for (MyMesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it)
{
mesh.property(vertexID, *v_it) = i;
++i;
}
//find a boundary half edge
MyMesh::HalfedgeHandle heh, heh_init;
for (MyMesh::HalfedgeIter h_it = mesh.halfedges_begin(); h_it != mesh.halfedges_end(); ++h_it)
{
if (mesh.is_boundary(*h_it))
{
heh_init = *h_it;
heh = heh_init;
break;
}
}
//mesh.property(vertexID, mesh.from_vertex_handle(heh));
//mesh.point(mesh.from_vertex_handle(heh));
//push first boundary vertex
meshBoundryStatus.push_back(meshBoundary(mesh.property(vertexID, mesh.from_vertex_handle(heh)), mesh.point(mesh.from_vertex_handle(heh))));
heh = mesh.next_halfedge_handle(heh);
// push all boundary vertex
while (heh != heh_init) {
meshBoundryStatus.push_back(meshBoundary(mesh.property(vertexID, mesh.from_vertex_handle(heh)), mesh.point(mesh.from_vertex_handle(heh))));
heh = mesh.next_halfedge_handle(heh);
}
//caculate length
OpenMesh::Vec3d vec;
for (i = 0; i < meshBoundryStatus.size() - 1; ++i)
{
vec = meshBoundryStatus[i + 1].position - meshBoundryStatus[i].position;
meshBoundryStatus[i].distanceToNext = vec.norm();
tLen += meshBoundryStatus[i].distanceToNext;
}
vec = meshBoundryStatus[0].position - meshBoundryStatus[i].position;
meshBoundryStatus[i].distanceToNext = vec.norm();
tLen += meshBoundryStatus[i].distanceToNext;
//get other information
mesh.request_vertex_status();
meshVetexNum = mesh.n_vertices();
mesh.request_face_status();
meshFaceNum = mesh.n_faces();
mesh.release_face_status();
mesh.release_vertex_status();
cout << "#vertices: " << meshVetexNum << endl;
cout << "#faces: " << meshFaceNum << endl;
cout << "#boundary vertices: " << meshBoundryStatus.size() << endl;
cout << "coner vertices: " << meshBoundryStatus[0].vertexID << " " << meshBoundryStatus[meshBoundryStatus.size()/3].vertexID<<" ";
cout << meshBoundryStatus[meshBoundryStatus.size() * 2 / 3].vertexID <<" "<< meshBoundryStatus[meshBoundryStatus.size() - 1].vertexID << endl;
cout << "Total length: " << tLen << endl;
//map interior vertex
//resize A and b
A.resize(meshVetexNum, meshVetexNum);
Bu.resize(meshVetexNum);
Bv.resize(meshVetexNum);
u.resize(meshVetexNum);
v.resize(meshVetexNum);
A.setZero();
Bu.setZero();
Bv.setZero();
vector<meshVertex> oneRing;
typedef Eigen::Triplet<double> T;
std::vector<T> tripletList;
for (MyMesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it)
{
oneRing.clear();
if (!mesh.is_boundary(*v_it))
{
for (MyMesh::VertexVertexIter vv_it = mesh.vv_iter(*v_it); vv_it.is_valid(); ++vv_it)
{
oneRing.push_back(meshVertex(mesh.property(vertexID, *vv_it), mesh.point(*vv_it)));
}
double wij = 0;
double sumWij = 0;
double cotaij = 0;
double cotbij = 0;
OpenMesh::Vec3d vi_12vi, vi_12v, vi12vi, vi12v;
for (int i = 0; i < oneRing.size(); ++i)
{
if (i == 0)
{
vi_12v = mesh.point(*v_it) - oneRing[oneRing.size() - 1].position;
vi_12vi = oneRing[0].position - oneRing[oneRing.size() - 1].position;
vi12v = mesh.point(*v_it) - oneRing[i + 1].position;
vi12vi = oneRing[i].position - oneRing[i + 1].position;
}
else
if (i == oneRing.size() - 1)
{
vi_12v = mesh.point(*v_it) - oneRing[i - 1].position;
vi_12vi = oneRing[i].position - oneRing[i - 1].position;
vi12v = mesh.point(*v_it) - oneRing[0].position;
vi12vi = oneRing[i].position - oneRing[0].position;
}
else
{
vi_12v = mesh.point(*v_it) - oneRing[i - 1].position;
vi_12vi = oneRing[i].position - oneRing[i - 1].position;
vi12v = mesh.point(*v_it) - oneRing[i + 1].position;
vi12vi = oneRing[i].position - oneRing[i + 1].position;
}
vi_12v.normalize();
vi_12vi.normalize();
vi12v.normalize();
vi12vi.normalize();
cotaij = cot(acos(dot(vi_12v, vi_12vi)));
cotbij = cot(acos(dot(vi12v, vi12vi)));
wij = 0.5 * (cotaij + cotbij);
sumWij += wij;
//insert
tripletList.push_back(T(mesh.property(vertexID, *v_it), oneRing[i].vertexID, wij));//insert wij (i != j)
}
tripletList.push_back(T(mesh.property(vertexID, *v_it), mesh.property(vertexID, *v_it), - sumWij));//when i = j
}
else
{
tripletList.push_back(T(mesh.property(vertexID, *v_it), mesh.property(vertexID, *v_it), 1));// add boundary
}
}
A.setFromTriplets(tripletList.begin(), tripletList.end());
//debug << A << endl;
//map boundary to a unit square
BoundaryMap();
Solve(solveType);
//change the mesh and output
if (outType == 1)
{
i = 0;
for (MyMesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it)
{
mesh.set_point(*v_it, MyMesh::Point(u[i], v[i], 0));
++i;
}
// write mesh to output.*
OpenMesh::IO::write_mesh(mesh, outfile);
}
else
{
mesh.request_vertex_texcoords2D();
i = 0;
for (MyMesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it)
{
mesh.set_texcoord2D(*v_it, MyMesh::TexCoord2D(u[i], v[i]));
++i;
}
// write mesh to output.*
OpenMesh::IO::Options wopt;
wopt += OpenMesh::IO::Options::VertexTexCoord;
OpenMesh::IO::write_mesh(mesh, outfile, wopt);
mesh.release_vertex_texcoords2D();
}
}
// ============================================================================
void
Solve(const Epetra_LinearProblem Problem)
{
Solve(Problem.GetMatrix(), Problem.GetLHS(), Problem.GetRHS());
}
bool CMOOSNavTopDownCalEngine::Calculate(double dfTimeNow)
{
AddToOutput("TDC: Attempting to calculate");
if(!MakePseudoBeacons())
return false;
AddToOutput("TDC: Pseudo Beacons Made OK");
if(!MakeObservations())
return false;
AddToOutput("TDC: made observations");
//send progress information..
IndicateGatherProgress();
//make an intial guess...
SeedSolution();
for(int nIteration = 0; nIteration<m_nLSQIterations; nIteration++)
{
if(MakeObsMatrices())
{
switch(Solve())
{
case LSQ_NO_SOLUTION:
// StatusMessage("FAILED TO SOLVE TDC");
MOOSTrace("FAILED TO SOLVE TDC");
return false;
case LSQ_IN_PROGRESS:
break;
case LSQ_SOLVED:
if(!DoWStatistic())
{
//we will have removed the observation
//that is not fitting...try to iterate again..
nIteration = 0;
break;
}
else
{
return OnSolved();
}
}
}
}
//if we have spect ages trying to figure it out we had better bail
if(m_nNoConvergenceCounter++>FAILED_CONVERGENCE_LIMIT)
{
AddToOutput("Could not solve for beacon (no convergence)... ");
}
return true;
}
inline void Solve(const string resume_file) { Solve(resume_file.c_str()); }
/// check out sawConstraintController
void updateKinematics(){
jointHandles_ = VrepRobotArmDriverP_->getJointHandles();
auto eigentestJacobian=::grl::vrep::getJacobian(*VrepRobotArmDriverP_);
/// The row/column major order is swapped between cisst and VREP!
this->currentKinematicsStateP_->Jacobian.SetSize(eigentestJacobian.cols(),eigentestJacobian.rows());
Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mckp2(this->currentKinematicsStateP_->Jacobian.Pointer(),this->currentKinematicsStateP_->Jacobian.cols(),this->currentKinematicsStateP_->Jacobian.rows());
mckp2 = eigentestJacobian.cast<double>();
//Eigen::Map<Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> > mf(eigentestJacobian,eigentestJacobian.cols(),eigentestJacobian.rows());
//Eigen::MatrixXf eigenJacobian = mf;
Eigen::MatrixXf eigenJacobian = eigentestJacobian;
///////////////////////////////////////////////////////////
// Copy Joint Interval, the range of motion for each joint
// lower limits
auto & llim = std::get<vrep::VrepRobotArmDriver::JointLowerPositionLimit>(currentArmState_);
std::vector<double> llimits(llim.begin(),llim.end());
jointPositionLimitsVFP_->LowerLimits = vctDoubleVec(llimits.size(),&llimits[0]);
// upper limits
auto & ulim = std::get<vrep::VrepRobotArmDriver::JointUpperPositionLimit>(currentArmState_);
std::vector<double> ulimits(ulim.begin(),ulim.end());
jointPositionLimitsVFP_->UpperLimits = vctDoubleVec(ulimits.size(),&ulimits[0]);
// current position
auto & currentJointPos = std::get<vrep::VrepRobotArmDriver::JointPosition>(currentArmState_);
std::vector<double> currentJointPosVec(currentJointPos.begin(),currentJointPos.end());
vctDoubleVec vctDoubleVecCurrentJoints(currentJointPosVec.size(),¤tJointPosVec[0]);
// update limits
/// @todo does this leak memory when called every time around?
UpdateJointPosLimitsVF(positionLimitsName,jointPositionLimitsVFP_->UpperLimits,jointPositionLimitsVFP_->LowerLimits,vctDoubleVecCurrentJoints);
const auto& handleParams = VrepRobotArmDriverP_->getVrepHandleParams();
Eigen::Affine3d currentEndEffectorPose =
getObjectTransform(
std::get<vrep::VrepRobotArmDriver::RobotTipName>(handleParams)
,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
);
auto currentEigenT = currentEndEffectorPose.translation();
auto& currentCisstT = currentKinematicsStateP_->Frame.Translation();
currentCisstT[0] = currentEigenT(0);
currentCisstT[1] = currentEigenT(1);
currentCisstT[2] = currentEigenT(2);
#ifndef IGNORE_ROTATION
Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> ccr(currentKinematicsStateP_->Frame.Rotation().Pointer());
ccr = currentEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
/// @todo set rotation component of current position
Eigen::Affine3d desiredEndEffectorPose =
getObjectTransform(
std::get<vrep::VrepRobotArmDriver::RobotTargetName>(handleParams)
,std::get<vrep::VrepRobotArmDriver::RobotTargetBaseName>(handleParams)
);
auto desiredEigenT = desiredEndEffectorPose.translation();
auto& desiredCisstT = desiredKinematicsStateP_->Frame.Translation();
desiredCisstT[0] = desiredEigenT(0);
desiredCisstT[1] = desiredEigenT(1);
desiredCisstT[2] = desiredEigenT(2);
#ifndef IGNORE_ROTATION
Eigen::Map<Eigen::Matrix<double,3,3,Eigen::ColMajor>> dcr(desiredKinematicsStateP_->Frame.Rotation().Pointer());
dcr = desiredEndEffectorPose.rotation();
#endif // IGNORE_ROTATION
/// @todo set rotation component of desired position
// for debugging, the translation between the current and desired position in cartesian coordinates
auto inputDesired_dx = desiredCisstT - currentCisstT;
vct3 dx_translation, dx_rotation;
// Rotation part
vctAxAnRot3 dxRot;
vct3 dxRotVec;
dxRot.FromNormalized((currentKinematicsStateP_->Frame.Inverse() * desiredKinematicsStateP_->Frame).Rotation());
dxRotVec = dxRot.Axis() * dxRot.Angle();
dx_rotation[0] = dxRotVec[0];
dx_rotation[1] = dxRotVec[1];
dx_rotation[2] = dxRotVec[2];
//dx_rotation.SetAll(0.0);
dx_rotation = currentKinematicsStateP_->Frame.Rotation() * dx_rotation;
Eigen::AngleAxis<float> tipToTarget_cisstToEigen;
Eigen::Matrix3f rotmat;
double theta = std::sqrt(dx_rotation[0]*dx_rotation[0]+dx_rotation[1]*dx_rotation[1]+dx_rotation[2]*dx_rotation[2]);
rotmat= Eigen::AngleAxisf(theta,Eigen::Vector3f(dx_rotation[0]/theta,dx_rotation[1]/theta,dx_rotation[2]/theta));
// std::cout << "\ntiptotarget \n" << tipToTarget.matrix() << "\n";
// std::cout << "\ntiptotargetcisst\n" << rotmat.matrix() << "\n";
//BOOST_LOG_TRIVIAL(trace) << "\n test desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
SetKinematics(*currentKinematicsStateP_); // replaced by name of object
// fill these out in the desiredKinematicsStateP_
//RotationType RotationMember; // vcRot3
//TranslationType TranslationMember; // vct3
SetKinematics(*desiredKinematicsStateP_); // replaced by name of object
// call setKinematics with the new kinematics
// sawconstraintcontroller has kinematicsState
// set the jacobian here
//////////////////////
/// @todo move code below here back under run_one updateKinematics() call
/// @todo need to provide tick time in double seconds and get from vrep API call
float simulationTimeStep = simGetSimulationTimeStep();
UpdateOptimizer(simulationTimeStep);
vctDoubleVec jointAngles_dt;
auto returncode = Solve(jointAngles_dt);
/// @todo check the return code, if it doesn't have a result, use the VREP version as a fallback and report an error.
if(returncode != nmrConstraintOptimizer::NMR_OK) BOOST_THROW_EXCEPTION(std::runtime_error("VrepInverseKinematicsController: constrained optimization error, please investigate"));
/// @todo: rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command?
std::string str;
// str = "";
for (std::size_t i=0 ; i < jointHandles_.size() ; i++)
{
float currentAngle;
auto ret = simGetJointPosition(jointHandles_[i],¤tAngle);
BOOST_VERIFY(ret!=-1);
float futureAngle = currentAngle + jointAngles_dt[i];
//simSetJointTargetVelocity(jointHandles_[i],jointAngles_dt[i]/simulationTimeStep);
//simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]);
//simSetJointTargetPosition(jointHandles_[i],futureAngle);
simSetJointPosition(jointHandles_[i],futureAngle);
str+=boost::lexical_cast<std::string>(jointAngles_dt[i]);
if (i<jointHandles_.size()-1)
str+=", ";
}
BOOST_LOG_TRIVIAL(trace) << "jointAngles_dt: "<< str;
auto optimizerCalculated_dx = this->currentKinematicsStateP_->Jacobian * jointAngles_dt;
BOOST_LOG_TRIVIAL(trace) << "\n desired dx: " << inputDesired_dx << " " << dx_rotation << "\noptimizer Calculated dx: " << optimizerCalculated_dx;
}
