double ProblemDescription::evaluateConstraint( const double * xi,
double * h,
double * H )
{
if ( factory.empty() ) {return 0; }
TIMER_START( "constraint" );
assert( h ); // make sure that h is not NULL
prepareData( xi );
MatMap h_map( h, factory.numOutput(), 1 );
if ( !H ){
const double val = factory.evaluate( trajectory, h_map );
TIMER_STOP( "constraint" );
return val;
}
MatMap H_map( H, trajectory.size(), factory.numOutput() );
double magnitude = factory.evaluate(trajectory, h_map, H_map );
if( doing_covariant ){
//TODO find out if this is correct
MatMap H_map2( H, trajectory.N(),
trajectory.M()*factory.numOutput() );
metric.multiplyLowerInverse( H_map2 );
}
TIMER_STOP( "constraint" );
return magnitude;
}
std::vector<TagMatch> detectTags(image_u8_t *im) {
const int hamm_hist_max = 10;
int hamm_hist[hamm_hist_max];
memset(hamm_hist, 0, sizeof(hamm_hist));
zarray_t *detections = apriltag_detector_detect(td, im);
std::vector<TagMatch> tag_matches;
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
if (!quiet) {
printf("detection %3d: id (%2dx%2d)-%-4d, hamming %d, goodness %8.3f, margin %8.3f\n",
i, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin);
// image_u8_draw_line(im, det->p[x][0], det->p[x][1], det->p[x+1][0], det->p[x+1][1], 255, 10);
}
if (tag_id == -1 || det->id == tag_id) {
TagMatch tag_match;
tag_match.id = det->family->d*det->family->d;
tag_match.p0 = cv::Point2d(det->p[0][0], det->p[0][1]);
tag_match.p1 = cv::Point2d(det->p[1][0], det->p[1][1]);
tag_match.p2 = cv::Point2d(det->p[2][0], det->p[2][1]);
tag_match.p3 = cv::Point2d(det->p[3][0], det->p[3][1]);
Eigen::Map<Eigen::Matrix3d> H_map(det->H->data);
tag_match.H = H_map.transpose();
tag_matches.push_back(tag_match);
}
hamm_hist[det->hamming]++;
}
apriltag_detections_destroy(detections);
if (!quiet) {
timeprofile_display(td->tp);
printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads);
printf("Hamming histogram: ");
for (int i = 0; i < hamm_hist_max; i++)
printf("%5d", hamm_hist[i]);
printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
printf("\n");
}
return tag_matches;
}
