std::vector< Proposals > LPO::propose(const ImageOverSegmentation& ios, float max_iou, int model_id, bool box_nms) const {
std::vector< Proposals > all_prop;
// Generate all proposals
for( int i=0; i<models_.size(); i++ )
if( i==model_id || model_id==-1 ){
const std::vector<Proposals> & props = models_[i]->propose( ios );
for( const Proposals & p: props ) {
// Can we merge some proposal maps?
bool merge = false;
for( Proposals & pp: all_prop ) {
if( pp.s == p.s ) {
eassert( pp.p.cols() == p.p.cols() );
// Merge the proposal maps
merge = true;
RMatrixXb new_p( pp.p.rows()+p.p.rows(), p.p.cols() );
new_p.topRows ( pp.p.rows() ) = pp.p;
new_p.bottomRows( p.p.rows() ) = p.p;
pp.p = new_p;
break;
}
}
if( !merge )
all_prop.push_back( p );
}
}
if( box_nms )
return boxNms( all_prop, max_iou );
// Remove empty proposals and (near) duplicates
// Only doing it on CRF proposals is much faster and doesn't generate
// too many more proposals (~100 more)
all_prop[0] = nms( all_prop[0], max_iou );
return all_prop;
// return nms( all_prop, max_iou );
}
multi_point<T> reproject_internal(multi_point<T> const & mp, proj_transform const& proj_trans, unsigned int & n_err)
{
multi_point<T> new_mp;
if (proj_trans.is_known())
{
// If the projection is known we do them all at once because it is faster
// since we know that no point will fail reprojection
new_mp.assign(mp.begin(), mp.end());
proj_trans.forward(new_mp);
}
else
{
new_mp.reserve(mp.size());
for (auto const& p : mp)
{
point<T> new_p(p);
if (!proj_trans.forward(new_p))
{
++n_err;
}
else
{
new_mp.emplace_back(std::move(new_p));
}
}
}
return new_mp;
}
/* Import the configuration (x,y,z) of particles.
* The center of mass is set to (0,0,0)
* INPUT
* importfilename: File name
* skipline: Line number of the header
*/
void System::importCluster(char* importfilename, int skipline){
sprintf( filename, "%s", importfilename);
string s_filename = filename;
int i_backslash = s_filename.find_last_of( "/") + 1;
int i_extention = s_filename.find( ".dat" );
sprintf(filename, "%s",
(s_filename.substr(i_backslash,i_extention-i_backslash)).c_str());
ifstream fin;
fin.open( importfilename );
double x, y, z;
char buf[1000];
for (int i = 0; i< skipline; i++){
fin.getline( buf, 1000 );
}
do{
fin >> x >> y >> z;
if( fin.fail() )
break;
vec3d new_p(x, y, z);
init_aggregate.push_back(new_p);
} while (!fin.eof());
shiftCenterOfMass( init_aggregate );
}
point<T> reproject_internal(point<T> const& p, proj_transform const& proj_trans, unsigned int & n_err)
{
point<T> new_p(p);
if (!proj_trans.forward(new_p))
{
++n_err;
}
return new_p;
}
void rename_dat_file(fs::path const p)
{
try
{
fs::path new_p(p);
new_p.replace_extension(".wor");
if (fs::exists(new_p))
fs::remove(new_p);
fs::rename(p, new_p);
}
catch (std::exception const& err)
{
PRINT_ERROR(fmt(_("cannot rename file %1%: %2%")) % p % err.what());
}
}
inline path change_extension( const path & p, const path & new_extension )
{
path new_p( p );
new_p.replace_extension( new_extension );
return new_p;
}
void CameraParameters::calibrate(
std::vector<GVector::vector3d<double> > &p_f,
std::vector<GVector::vector2d<double> > &p_i, int cal_type) {
assert(p_f.size() == p_i.size());
assert(cal_type != 0);
p_to_est.clear();
p_to_est.push_back(FOCAL_LENGTH);
p_to_est.push_back(Q_1);
p_to_est.push_back(Q_2);
p_to_est.push_back(Q_3);
p_to_est.push_back(T_1);
p_to_est.push_back(T_2);
int num_alpha(0);
if (cal_type & FULL_ESTIMATION) {
int count_alpha(0); //The number of well detected line segment points
std::vector<CalibrationData>::iterator ls_it = calibrationSegments.begin();
for (; ls_it != calibrationSegments.end(); ls_it++) {
std::vector< std::pair<GVector::vector2d<double>,bool> >::iterator
pts_it = (*ls_it).imgPts.begin();
for (; pts_it != (*ls_it).imgPts.end(); pts_it++) {
if (pts_it->second) count_alpha ++;
}
}
if (count_alpha > 0) {
p_alpha = Eigen::VectorXd(count_alpha);
count_alpha = 0;
ls_it = calibrationSegments.begin();
for (; ls_it != calibrationSegments.end(); ls_it++) {
std::vector< std::pair<GVector::vector2d<double>,bool> >::iterator
pts_it = (*ls_it).imgPts.begin();
std::vector< double >::iterator alphas_it = (*ls_it).alphas.begin();
for (; pts_it != (*ls_it).imgPts.end() &&
alphas_it != (*ls_it).alphas.end(); pts_it++, alphas_it++) {
if (pts_it->second) {
p_alpha(count_alpha++) = (*alphas_it);
}
}
}
}
p_to_est.push_back(PP_X);
p_to_est.push_back(PP_Y);
p_to_est.push_back(DIST);
num_alpha = count_alpha;
}
double lambda(0.01);
Eigen::VectorXd p(STATE_SPACE_DIMENSION + num_alpha);
p.setZero();
// Calculate first chisqr for all points using the start parameters
double old_chisqr = calc_chisqr(p_f, p_i, p, cal_type);
#ifndef NDEBUG
std::cerr << "Chi-square: "<< old_chisqr << std::endl;
#endif
// Create and fill corner measurement covariance matrix
Eigen::Matrix2d cov_corner_inv;
cov_corner_inv <<
1 / additional_calibration_information->cov_corner_x->getDouble(), 0 , 0 ,
1 / additional_calibration_information->cov_corner_y->getDouble();
// Create and fill line segment measurement covariance matrix
Eigen::Matrix2d cov_ls_inv;
cov_ls_inv << 1 / additional_calibration_information->cov_ls_x->getDouble(),
0 , 0 , 1 / additional_calibration_information->cov_ls_y->getDouble();
// Matrices for A, b and the Jacobian J
Eigen::MatrixXd alpha(STATE_SPACE_DIMENSION + num_alpha,
STATE_SPACE_DIMENSION + num_alpha);
Eigen::VectorXd beta(STATE_SPACE_DIMENSION + num_alpha, 1);
Eigen::MatrixXd J(2, STATE_SPACE_DIMENSION + num_alpha);
bool stop_optimization(false);
int convergence_counter(0);
double t_start=GetTimeSec();
while (!stop_optimization) {
// Calculate Jacobi-Matrix, alpha and beta
// Iterate over alle point pairs
std::vector<GVector::vector3d<double> >::iterator it_p_f = p_f.begin();
std::vector<GVector::vector2d<double> >::iterator it_p_i = p_i.begin();
double epsilon = sqrt(std::numeric_limits<double>::epsilon());
alpha.setZero();
beta.setZero();
for (; it_p_f != p_f.end(); it_p_f++, it_p_i++) {
J.setZero();
GVector::vector2d<double> proj_p;
field2image(*it_p_f, proj_p, p);
proj_p = proj_p - *it_p_i;
std::vector<int>::iterator it = p_to_est.begin();
for (; it != p_to_est.end(); it++) {
int i = *it;
Eigen::VectorXd p_diff = p;
p_diff(i) = p_diff(i) + epsilon;
GVector::vector2d<double> proj_p_diff;
field2image(*it_p_f, proj_p_diff, p_diff);
J(0,i) = ((proj_p_diff.x - (*it_p_i).x) - proj_p.x) / epsilon;
J(1,i) = ((proj_p_diff.y - (*it_p_i).y) - proj_p.y) / epsilon;
}
alpha += J.transpose() * cov_corner_inv * J;
beta += J.transpose() * cov_corner_inv *
Eigen::Vector2d(proj_p.x, proj_p.y);
}
if (cal_type & FULL_ESTIMATION) {
// First, calculate how many alpha we need to estimate
std::vector<CalibrationData>::iterator ls_it =
calibrationSegments.begin();
int i = 0;
for (; ls_it != calibrationSegments.end(); ls_it++) {
std::vector< std::pair<GVector::vector2d<double>,bool> >::iterator
pts_it = (*ls_it).imgPts.begin();
for (; pts_it != (*ls_it).imgPts.end(); pts_it++) {
if (pts_it->second) {
GVector::vector2d<double> proj_p;
GVector::vector3d<double >alpha_point;
if (ls_it->straightLine) {
alpha_point = p_alpha(i) * (*ls_it).p1 + (1 - p_alpha(i)) * (*ls_it).p2;
} else {
double theta = p_alpha(i) * (*ls_it).theta1 + (1.0 - p_alpha(i)) * (*ls_it).theta2;
alpha_point = ls_it->center + ls_it->radius*GVector::vector3d<double>(cos(theta),sin(theta),0.0);
}
field2image(alpha_point, proj_p, p);
proj_p = proj_p - (*pts_it).first;
J.setZero();
std::vector<int>::iterator it = p_to_est.begin();
for (; it != p_to_est.end(); it++) {
int j = *it;
Eigen::VectorXd p_diff = p;
p_diff(j) = p_diff(j) + epsilon;
GVector::vector2d<double> proj_p_diff;
field2image(alpha_point, proj_p_diff, p_diff);
J(0,j) = ((proj_p_diff.x - (*pts_it).first.x) - proj_p.x) /
epsilon;
J(1,j) = ((proj_p_diff.y - (*pts_it).first.y) - proj_p.y) /
epsilon;
}
double my_alpha = p_alpha(i) + epsilon;
if (ls_it->straightLine) {
alpha_point = my_alpha * (*ls_it).p1 + (1 - my_alpha) *
(*ls_it).p2;
} else {
double theta = my_alpha * (*ls_it).theta1 + (1.0 - my_alpha) *
(*ls_it).theta2;
alpha_point = ls_it->center +
ls_it->radius*GVector::vector3d<double>(cos(theta),
sin(theta),0.0);
}
GVector::vector2d<double> proj_p_diff;
field2image(alpha_point, proj_p_diff);
J(0,STATE_SPACE_DIMENSION + i) =
((proj_p_diff.x - (*pts_it).first.x) - proj_p.x) / epsilon;
J(1,STATE_SPACE_DIMENSION + i) =
((proj_p_diff.y - (*pts_it).first.y) - proj_p.y) / epsilon;
alpha += J.transpose() * cov_ls_inv * J;
beta += J.transpose() * cov_ls_inv *
Eigen::Vector2d(proj_p.x, proj_p.y);
i++;
}
}
}
}
// Augment alpha
alpha += Eigen::MatrixXd::Identity(
STATE_SPACE_DIMENSION + num_alpha, STATE_SPACE_DIMENSION + num_alpha)
* lambda;
// Solve for x
Eigen::VectorXd new_p(STATE_SPACE_DIMENSION + num_alpha);
// Due to an API change we need to check for
// the right call at compile time
#ifdef EIGEN_WORLD_VERSION
// alpha.llt().solve(-beta, &new_p); -- modify 1/15/16
// -- move to Eigen3 structure -
// -- http://eigen.tuxfamily.org/dox/Eigen2ToEigen3.html
new_p = alpha.llt().solve(-beta);
#else
Eigen::Cholesky<Eigen::MatrixXd> c(alpha);
new_p = c.solve(-beta);
#endif
// Calculate chisqr again
double chisqr = calc_chisqr(p_f, p_i, new_p, cal_type);
if (chisqr < old_chisqr) {
focal_length->setDouble(focal_length->getDouble() + new_p[FOCAL_LENGTH]);
principal_point_x->setDouble(
principal_point_x->getDouble() + new_p[PP_X]);
principal_point_y->setDouble(
principal_point_y->getDouble() + new_p[PP_Y]);
distortion->setDouble(distortion->getDouble() + new_p[DIST]);
tx->setDouble(tx->getDouble() + new_p[T_1]);
ty->setDouble(ty->getDouble() + new_p[T_2]);
tz->setDouble(tz->getDouble() + new_p[T_3]);
Quaternion<double> q_diff;
GVector::vector3d<double> aa_diff(new_p[Q_1], new_p[Q_2], new_p[Q_3]);
q_diff.setAxis(aa_diff.norm(), aa_diff.length());
Quaternion<double> q_field2cam = Quaternion<double>(
q0->getDouble(),q1->getDouble(),q2->getDouble(),q3->getDouble());
q_field2cam.norm();
q_field2cam = q_diff * q_field2cam ;
q0->setDouble(q_field2cam.x);
q1->setDouble(q_field2cam.y);
q2->setDouble(q_field2cam.z);
q3->setDouble(q_field2cam.w);
for (int i=0; i < num_alpha; i++)
p_alpha[i] += new_p[STATE_SPACE_DIMENSION + i];
// Normalize focal length an orientation when the optimization tends to go into the wrong
// of both possible projections
if (focal_length->getDouble() < 0) {
focal_length->setDouble(-focal_length->getDouble());
q_field2cam = q_rotate180 * q_field2cam;
q0->setDouble(q_field2cam.x);
q1->setDouble(q_field2cam.y);
q2->setDouble(q_field2cam.z);
q3->setDouble(q_field2cam.w);
}
if (old_chisqr - chisqr < 0.001) {
stop_optimization = true;
} else {
lambda /= 10;
convergence_counter = 0;
}
old_chisqr = chisqr;
#ifndef NDEBUG
std::cerr << "Chi-square: "<< old_chisqr << std::endl;
#endif
} else {
lambda *= 10;
if (convergence_counter++ > 10) stop_optimization = true;
}
if ((GetTimeSec() - t_start) >
additional_calibration_information->convergence_timeout->getDouble()) {
stop_optimization=true;
}
}
// Debug output starts here
#ifndef NDEBUG
// Estimated parameters
std::cerr << "Estimated parameters: " << std::endl;
std::cerr << focal_length->getDouble() << " "
<< principal_point_x->getDouble() << " "
<< principal_point_y->getDouble() << " " << distortion->getDouble()
<< std::endl;
std::cerr << q0->getDouble() << " " << q1->getDouble() << " "
<< q2->getDouble() << " " << q3->getDouble() << std::endl;
std::cerr << tx->getDouble() << " " << ty->getDouble() << " "
<< tz->getDouble() << std::endl;
std::cerr << "alphas: " << p_alpha << std::endl;
// Testing calibration by projecting the four field points into the image
// plane and calculate MSE
std::vector<GVector::vector3d<double> >::iterator it_p_f = p_f.begin();
std::vector<GVector::vector2d<double> >::iterator it_p_i = p_i.begin();
double corner_x(0);
double corner_y(0);
for (; it_p_f != p_f.end(); it_p_f++, it_p_i++) {
GVector::vector2d<double> proj_p;
field2image(*it_p_f, proj_p);
GVector::vector3d<double> some_point;
image2field(some_point,proj_p, 150);
std::cerr << "Point in world: ("<< it_p_f->x << "," << it_p_f->y << ","
<< it_p_f->z << ")" << std::endl;
std::cerr << "Point should be at (" << it_p_i->x << "," << it_p_i->y
<< ") and is projected at (" << proj_p.x << "," << proj_p.y <<")"
<< std::endl;
corner_x += (proj_p.x - it_p_i->x) * (proj_p.x - it_p_i->x);
corner_y += (proj_p.y - it_p_i->y) * (proj_p.y - it_p_i->y);
}
std::cerr << "RESIDUAL CORNER POINTS: " << sqrt(corner_x/4) << " "
<< sqrt(corner_y/4) << std::endl;
if (cal_type & FULL_ESTIMATION) {
// Testing calibration by projecting the points on the lines into the image
// plane and calculate MSE
double line_x(0);
double line_y(0);
std::vector<CalibrationData>::iterator ls_it = calibrationSegments.begin();
int i = 0;
for (; ls_it != calibrationSegments.end(); ls_it++) {
std::vector< std::pair<GVector::vector2d<double>,bool> >::iterator pts_it =
(*ls_it).imgPts.begin();
for (; pts_it != (*ls_it).imgPts.end(); pts_it++) {
if (pts_it->second) {
GVector::vector2d<double> proj_p;
double alpha = p_alpha(i);
GVector::vector3d<double >alpha_point;
if (ls_it->straightLine) {
alpha_point = alpha * (*ls_it).p1 + (1 - alpha) * (*ls_it).p2;
} else {
double theta = alpha * (*ls_it).theta1 + (1.0 - alpha) *
(*ls_it).theta2;
alpha_point = ls_it->center +
ls_it->radius*GVector::vector3d<double>(
cos(theta),sin(theta),0.0);
}
field2image(alpha_point, proj_p, p);
line_x += (proj_p.x - pts_it->first.x) * (proj_p.x - pts_it->first.x);
line_y += (proj_p.y - pts_it->first.y) * (proj_p.y - pts_it->first.y);
i++;
}
}
}
if (i != 0) {
std::cerr << "RESIDUAL LINE POINTS: " << sqrt(line_x/(double)i) << " "
<< sqrt(line_y/(double)i) << std::endl;
}
}
#endif
}
void outopt(int ch, int count)
{
if (deadloop) {
if (ch == '[') deadloop++;
if (ch == ']') deadloop--;
return;
}
if (ch == '[') {
if (tape->is_set && tape->v == 0) {
deadloop++;
return;
}
}
switch(ch)
{
case '[': case ']': case '!': case '~': case 'X': case '#':
case 'I': case 'E':
if (ch == '!') {
flush_tape(1,0);
tape->cleaned = tape->is_set = first_run = !disable_init_optim;
} else if (ch == '~' && enable_optim && !disable_init_optim)
flush_tape(1,0);
else
flush_tape(0,0);
if (ch) outcmd(ch, count);
/* Loops end with zero */
if (ch == ']') {
tape->is_set = 1;
tape->v = 0;
tape->cleaned = 1;
tape->cleaned_val = tape->v;
}
/* I could save the cleaned tape state at the beginning of a loop,
* then when we find the matching end loop the two tapes could be
* merged to give a tape of known values after the loop ends.
* This would not break the pipeline style of this code.
*
* This would also give states where a cell is known to have one
* of two or more different values. */
return;
case '.':
if (tape->is_set) {
int c = tape->v;
if (bytecell) c &= 0xFF;
if (c > 0 && c < 128) {
add_string(c);
/* Limit the buffer size. */
if (sav_str_len >= 128*1024 - (tape->v=='\n')*1024)
flush_string();
break;
}
}
flush_tape(0,1);
outcmd(ch, count);
return;
case ',':
flush_tape(0,1);
clear_cell(tape);
outcmd(ch, count);
return;
case '>': while(count-->0) { if (tape->n == 0) new_n(tape); tape=tape->n; curroff++; } break;
case '<': while(count-->0) { if (tape->p == 0) new_p(tape); tape=tape->p; curroff--; } break;
case '+':
if (be_interface.cells_are_ints || bytecell) {
tape->v += count;
if (bytecell) tape->v %= 256; /* -255..255 */
} else {
int ov=0, res;
res = ov_iadd(tape->v, count, &ov);
if (!ov)
tape->v = res;
else {
flush_tape(0,1);
clear_cell(tape);
outcmd(ch, count);
}
}
break;
case '-':
if (be_interface.cells_are_ints || bytecell) {
tape->v -= count;
if (bytecell) tape->v %= 256; /* -255..255 */
} else {
int ov=0, res;
res = ov_isub(tape->v, count, &ov);
if (!ov)
tape->v = res;
else {
flush_tape(0,1);
clear_cell(tape);
outcmd(ch, count);
}
}
break;
case '=': tape->v = count; tape->is_set = 1; break;
case 'B':
flush_tape(0,1);
if (be_interface.disable_be_optim) be_codegen_failure();
outcmd(ch, count);
return;
case 'M': case 'N': case 'S': case 'T':
if (ch == 'N') count = -count;
else if (ch == 'S') count = 1;
else if (ch == 'T') count = -1;
if (tape->is_set && tape->v == 0) {
tape->is_set = 0 ; tape->v = 0;
ch = 'C';
if (count == 1) ch = 'V';
else if (count == -1) { ch = 'W'; count = -count; }
else if (count < 0) { ch = 'D'; count = -count; }
} else {
ch = 'M';
if (count == 1) ch = 'S';
else if (count == -1) { ch = 'T'; count = -count; }
else if (count < 0) { ch = 'N'; count = -count; }
}
flush_tape(0,1);
clear_cell(tape);
if (be_interface.disable_be_optim) be_codegen_failure();
outcmd(ch, count);
return;
default:
if (be_interface.disable_be_optim) be_codegen_failure();
if (ch>=0 && ch<256)
fprintf(stderr, "Unknown token in bf2const.c (%d)\n", ch);
flush_tape(0,0);
outcmd(ch, count);
return;
}
}
void outopt(int ch, int count)
{
if (deadloop) {
if (ch == '[') deadloop++;
if (ch == ']') deadloop--;
return;
}
if (ch == '[' && enable_mov_optim) {
if (tape->is_set && tape->v == 0) {
deadloop++;
return;
}
}
switch(ch)
{
default:
if (ch == '!') {
flush_tape(1,0);
tape->cleaned = tape->is_set = first_run = !disable_init_optim;
} else if (ch == '~' && enable_optim && !disable_init_optim)
flush_tape(1,0);
else
flush_tape(0,0);
if (ch) outcmd(ch, count);
/* Loops end with zero */
if (ch == ']') {
tape->is_set = 1;
tape->v = 0;
tape->cleaned = 1;
tape->cleaned_val = tape->v;
}
/* I could save the cleaned tape state at the beginning of a loop,
* then when we find the matching end loop the two tapes could be
* merged to give a tape of known values after the loop ends.
* This would not break the pipeline style of this code.
*
* This would also give states where a cell is known to have one
* of two or more different values. */
return;
case '.':
if (!disable_savestring && enable_be_optim &&
tape->is_set && tape->v > 0 && tape->v < 128) {
add_string(tape->v);
if (sav_str_len >= 128*1024) /* Limit the buffer size. */
{
add_string(0);
outcmd('"', 0);
sav_str_len = 0;
}
break;
}
flush_tape(0,1);
outcmd(ch, count);
return;
case ',':
flush_tape(0,1);
clear_cell(tape);
outcmd(ch, count);
return;
case '>': while(count-->0) { if (tape->n == 0) new_n(tape); tape=tape->n; curroff++; } break;
case '<': while(count-->0) { if (tape->p == 0) new_p(tape); tape=tape->p; curroff--; } break;
case '+': tape->v += count; break;
case '-': tape->v -= count; break;
case '=': tape->v = count; tape->is_set = 1; break;
case 'B':
/* Some BE are not 32 bits, try to avoid cell size mistakes */
if (!cells_are_ints && (tape->v > 65536 || tape->v < -65536))
;
else
if (tape->is_set) {
if (bytecell) tape->v %= 256; /* Note: preserves sign but limits range. */
reg_known = 1;
reg_val = tape->v;
break;
}
flush_tape(0,1);
reg_known = 0; reg_val = 0;
if (enable_be_optim) {
outcmd(ch, count);
} else {
outcmd('[', 1);
}
return;
case 'M':
case 'N':
case 'S':
case 'Q':
case 'm':
case 'n':
case 's':
case 'E':
if (!reg_known) {
flush_tape(0,1);
clear_cell(tape);
if (enable_be_optim) {
outcmd(ch, count);
} else switch(ch) {
case 'M': case 'm':
outcmd('+', count);
break;
case 'N': case 'n':
outcmd('-', count);
break;
case 'S': case 's':
outcmd('+', 1);
break;
case 'Q':
outcmd('[', 1);
outcmd('-', 1);
outcmd(']', 1);
if (count)
outcmd('+', count);
break;
case 'E':
outcmd(']', 1);
break;
}
return;
}
switch(ch) {
case 'm':
case 'M':
tape->v += reg_val * count;
break;
case 'n':
case 'N':
tape->v -= reg_val * count;
break;
case 's':
case 'S':
tape->v += reg_val;
break;
case 'Q':
if (reg_val != 0) {
tape->v = count;
tape->is_set = 1;
}
break;
}
if (bytecell) tape->v %= 256; /* Note: preserves sign but limits range. */
}
}
