void MainWindow::OnJiaoZhun() { #ifdef ARM QFile pointercal("/etc/pointercal"); if (pointercal.exists()) { pointercal.copy("/etc/pointercal.bak"); pointercal.remove(); } if (!pointercal.exists() || pointercal.size() == 0) { for (;;) { Calibration cal; cal.exec(); QMessageBox message(QMessageBox::Question, QString::fromUtf8("提示"), QString::fromUtf8("<p>请确认触摸屏已经校正完毕。</p>" "<p>如果你不确认此提示消息,将在10秒钟后重新进入触摸屏设置程序。</p>"), QMessageBox::Yes | QMessageBox::No); QTimer::singleShot(10 * 1000, &message, SLOT(reject())); int reply = message.exec(); if (reply == QMessageBox::Yes) { ::sync(); break; } } } #endif }
RealPoint3D<float> projectPoint(const Calibration& calibration, bool reduce2, int camera, const View& point) { if(point.inCamera(camera)) { RealPoint2D normalPoint; if(reduce2) normalPoint = RealPoint2D(2.0f*point[camera].u(), 2.0f*point[camera].v()); else normalPoint = RealPoint2D(point[camera].u(), point[camera].v()); RealPoint2D rectifiedPoint = calibration.rectify(camera, normalPoint); RealPoint3D<float> ray = calibration.ray(camera, rectifiedPoint); return RealPoint3D<float>(ray.x(), ray.y(), ray.z()); } else { return RealPoint3D<float>(); } }
void EpiMatcher::operator()(const Calibration& calib1, unsigned int camera1, const Calibration& calib2, unsigned int camera2, const vector<ExtendedPoint2D>& pi1, const vector<ExtendedPoint2D>& pi2, vector<pair<size_t, size_t> >& matches) { matches.clear(); const HomogenousPoint3D ch((float)calib2.translation(camera2).x(), (float)calib2.translation(camera2).y(), (float)calib2.translation(camera2).z(), 1.0f); HomogenousPoint2D e; mulMat43Vect4(calib1.projectionMatrix(camera1), ch, e); // Creating cross check vectors vector<pair<float, int> > prevPIMatch_(pi1.size(), make_pair(0.0, -1)); vector<pair<float, int> > curPIMatch_(pi2.size(), make_pair(0.0, -1)); #pragma omp parallel for for(size_t p = 0; p < pi2.size(); ++p) { const ExtendedPoint2D& point2 = pi2[p]; // Preselecting candidates using epipolar constraint vector<size_t> candidates; selectingPoints(e, calib2, camera2, calib1, camera1, point2, pi1, candidates); // Selecting the best point using ZNCC corelation size_t bestPoint = 0; float bestScore = 0.0; for(vector<size_t>::const_iterator it = candidates.begin(); it != candidates.end(); it++) { const float score = point2.descriptor().corelation(pi1[*it].descriptor()); if(score > bestScore) { bestPoint = *it; bestScore = score; } } if(bestScore >= scoreMin_) { #pragma omp critical { //matches.push_back(make_pair(bestPoint, p)); curPIMatch_[p] = make_pair(bestScore, bestPoint); if(prevPIMatch_[bestPoint].first <= bestScore) prevPIMatch_[bestPoint] = make_pair(bestScore, p); } } } // Matching points matches.clear(); for(size_t i = 0; i < curPIMatch_.size(); ++i) { if(curPIMatch_[i].second != -1) { if(prevPIMatch_[curPIMatch_[i].second].second == i) { matches.push_back(make_pair(curPIMatch_[i].second, i)); } } } }
void testApp::draw() { ofBackground(128); cam.begin(); glEnable(GL_DEPTH_TEST); glPushMatrix(); glScaled(1, -1, -1); ofDrawAxis(100); ofSetColor(255); curColor.draw(0, 0, curColor.getWidth(), curColor.getHeight()); glEnableClientState(GL_VERTEX_ARRAY); glEnableClientState(GL_COLOR_ARRAY); glColorPointer(3, GL_FLOAT, sizeof(Point3f), &(pointCloudColors[0].x)); glVertexPointer(3, GL_FLOAT, sizeof(Point3f), &(pointCloud[0].x)); glDrawArrays(GL_POINTS, 0, pointCloud.size()); glDisableClientState(GL_COLOR_ARRAY); glDisableClientState(GL_VERTEX_ARRAY); glDisable(GL_DEPTH_TEST); ofSetColor(255); glEnableClientState(GL_VERTEX_ARRAY); glVertexPointer(2, GL_FLOAT, sizeof(Point2f), &(imagePoints[0].x)); glDrawArrays(GL_POINTS, 0, pointCloud.size()); glDisableClientState(GL_VERTEX_ARRAY); Calibration* curCalibration; if(mouseX < ofGetWidth() / 2) { curCalibration = &kinectCalibration; } else { curCalibration = &colorCalibration; applyMatrix(makeMatrix(rotationColorToKinect, translationColorToKinect)); } curCalibration->draw3d(curImage); glPopMatrix(); cam.end(); }
void ProbeCalibrationWidget::calibrate() { // gnerate the transformation (rotation and translation) matrixes for each image std::cout<<std::endl; std::cout << "Calculating Image Data" << std::endl; std::cout << std::endl; std::vector<vnl_matrix<double>* > transformationSet(imageStack.size()); Calibration * calibrator = Calibration::New(); calibrator->ClearTransformations(); calibrator->ClearImagePoints(); for (uint i = 0; i < imageStack.size(); i++) { std::cout<<"Image "<<i+1<<" data"<<std::endl; vnl_quaternion<double> quaternion(rotations[i][1], rotations[i][2], rotations[i][3], rotations[i][0]); vnl_matrix<double> transformation = quaternion.rotation_matrix_transpose(); transformation = transformation.transpose(); calibrator->InsertTransformations(transformation, translations.get_row(i)); calibrator->InsertImagePoints(coords[i]); } calibrator->Calibrate(); calibrationParameters = calibrator->getEstimatedUSCalibrationParameters(); }
MARG::MARG(const MARGConfiguration& configuration, const Readout::MARG& readout, const Temperature& temperature_difference, const Calibration& calibration) { #ifdef USE_TEMPERATURE_COMPENSATION const int16_t temperature_offset[2] = { (int16_t)((temperature_difference.degreesCelsius() * LSM9DS0_OFFSET_PER_CELSIUS_G) / configuration.gScaleMdps()), (int16_t)((temperature_difference.degreesCelsius() * LSM9DS0_OFFSET_PER_CELSIUS_A) / configuration.aScaleMg()) }; const float temperature_scale[2] = { (float)temperature_difference.degreesCelsius() * sensitivity_per_celsius_g, (float)temperature_difference.degreesCelsius() * sensitivity_per_celsius_a }; #else const int16_t temperature_offset[2] = { 0, 0 }; const float temperature_scale[2] = { 1, 1 }; #endif const Vector<int16_t> raw_offset_adjusted[2] = { readout.g - calibration.g_offset - temperature_offset[0], readout.a - calibration.a_offset - temperature_offset[1] }; const Vector<float> scale_adjusted[2] = { calibration.g_scale * (1 + temperature_scale[0]), calibration.a_scale * (1 + temperature_scale[1]) }; g_ = toVector(raw_offset_adjusted[0] * scale_adjusted[0], configuration.gScaleMdps() / 1000 * M_PI / 180); a_ = toVector(raw_offset_adjusted[1] * scale_adjusted[1], configuration.aScaleMg() / 1000); m_ = toVector(readout.m * calibration.m_rotation - calibration.m_offset, configuration.mScaleMgauss() / 1000); if (calibration.hasInvertedPlacement()) { a_.setX(-a_.x()); a_.setY(-a_.y()); } if (!calibration.hasInvertedPlacement()) { g_.setX(-g_.x()); g_.setY(-g_.y()); } g_.setX(-g_.x()); g_.setY(-g_.y()); g_.setZ(-g_.z()); }
void saveTransformation(Calibration& from, Calibration& to, string filename) { Mat rotation, translation; from.getTransformation(to, rotation, translation); FileStorage fs(ofToDataPath(filename), FileStorage::WRITE); fs << "rotation" << rotation; fs << "translation" << translation; cout << "rotation:" << endl << rotation << endl; cout << "translation:" << endl << translation << endl; }
void createTriplets(const Bundle2& bundle, const Calibration& calibration, unsigned int curFrame, const vector<ShudaTriplet3D>& prevTriplets, vector<ShudaTriplet3D>& triplets) { triplets.clear(); bool usedTriplet[bundle.tracksCount()]; for(size_t i = 0; i < bundle.tracksCount(); ++i) usedTriplet[i] = false; for(size_t i = 0; i < bundle.frame(curFrame).size(); ++i) { for(int camera = 0; camera < calibration.num_cameras(); ++camera) { Track& t = *(bundle.frame(curFrame)[i].track()); if(bundle.frame(curFrame)[i].inTrackNumber() + 2 < t.size()) { ShudaTriplet3D triplet; triplet.frame = curFrame; triplet.camera = camera; bool ok; triplet.p1_2d = extractPoint(camera, bundle.frame(curFrame)[i], ok); if(!ok) continue; triplet.p1 = projectPoint(calibration, bundle.parameters().reduce2, camera, bundle.frame(curFrame)[i]); triplet.p2_2d = extractPoint(camera, t[bundle.frame(curFrame)[i].inTrackNumber() + 1], ok); if(!ok) continue; triplet.p2 = projectPoint(calibration, bundle.parameters().reduce2, camera, t[bundle.frame(curFrame)[i].inTrackNumber() + 1]); triplet.p3_2d = extractPoint(camera, t[bundle.frame(curFrame)[i].inTrackNumber() + 2], ok); if(!ok) continue; triplet.p3 = projectPoint(calibration, bundle.parameters().reduce2, camera, t[bundle.frame(curFrame)[i].inTrackNumber() + 2]); bool found = false; size_t j = 0; while(!found && (j < prevTriplets.size())) { if(prevTriplets[j].p2 == triplet.p1) found = true; else ++j; } if(found) { bool trunc = false; if(!usedTriplet[j] && !trunc) { triplet.prevTriplet = j; usedTriplet[j] = true; } else { triplet.prevTriplet = -1; } } else triplet.prevTriplet = -1; triplets.push_back(triplet); } } } }
void EpiMatcher::selectingPoints(const HomogenousPoint2D& e, const Calibration& calib1, unsigned int cam1, const Calibration& calib2, unsigned int cam2, const ExtendedPoint2D& point, const vector<ExtendedPoint2D>& pi, vector<size_t>& candidates) const { const RealPoint2D epiline_p1 = e; HomogenousPoint2D rect_p2 = calib1.rectify(cam1, point); HomogenousPoint3D tmp; mulMat34Vect3(calib1.invProjectionMatrix(cam1), rect_p2, tmp); HomogenousPoint2D tmp_p2; mulMat43Vect4(calib2.projectionMatrix(cam2), tmp, tmp_p2); const RealPoint2D epiline_p2 = tmp_p2; const float epi_x = epiline_p2.u() - epiline_p1.u(); const float epi_y = epiline_p2.v() - epiline_p1.v(); const float denum = std::sqrt(epi_x*epi_x + epi_y*epi_y); candidates.clear(); for(size_t i = 0; i < pi.size(); ++i) { const RealPoint2D p1 = calib2.rectify(cam2, pi[i]); const float dist = std::abs(epi_x*(epiline_p1.v() - p1.v()) - (epiline_p1.u() - p1.u())*epi_y)/denum; if((double)dist <= maxDist_ && pi[i].distance(point) <= zoneRadius_) candidates.push_back(i); } }
void Calibration::getTransformation(Calibration& dst, Mat& rotation, Mat& translation) { if(imagePoints.size() == 0 || dst.imagePoints.size() == 0) { ofLog(OF_LOG_ERROR, "getTransformation() requires both Calibration objects to have just been calibrated"); return; } if(imagePoints.size() != dst.imagePoints.size() || patternSize != dst.patternSize) { ofLog(OF_LOG_ERROR, "getTransformation() requires both Calibration objects to be trained simultaneously on the same board"); return; } Mat fundamentalMatrix, essentialMatrix; Mat cameraMatrix = distortedIntrinsics.getCameraMatrix(); Mat dstCameraMatrix = dst.getDistortedIntrinsics().getCameraMatrix(); // uses CALIB_FIX_INTRINSIC by default stereoCalibrate(objectPoints, imagePoints, dst.imagePoints, cameraMatrix, distCoeffs, dstCameraMatrix, dst.distCoeffs, distortedIntrinsics.getImageSize(), rotation, translation, essentialMatrix, fundamentalMatrix); }
void createFirstTriplets(const Bundle2& bundle, const Calibration& calibration, vector<ShudaTriplet3D>& triplets) { for(size_t i = 0; i < bundle.frame(0).size(); ++i) { for(int camera = 0; camera < calibration.num_cameras(); ++camera) { ShudaTriplet3D triplet; triplet.prevTriplet = -1; triplet.frame = 0; triplet.camera = camera; bool ok; Track& t = *(bundle.frame(0)[i].track()); triplet.p1_2d = extractPoint(camera, bundle.frame(0)[i], ok); if(!ok) continue; triplet.p1 = projectPoint(calibration, bundle.parameters().reduce2, camera, bundle.frame(0)[i]); triplet.p2_2d = extractPoint(camera, t[1], ok); if(!ok) continue; triplet.p2 = projectPoint(calibration, bundle.parameters().reduce2, camera, t[1]); triplet.p3_2d = extractPoint(camera, t[2], ok); if(!ok) continue; triplet.p3 = projectPoint(calibration, bundle.parameters().reduce2, camera, t[2]); triplets.push_back(triplet); } } }
/// Render calibration void Tuning::renderCalibration(Calibration& _calib) const { _calib.render(*this); }
int main() { int x, y , z; int leftX = 0; int leftY = 0; int rightX = 0; int rightY = 0; Mat cam0P, cam1P; Mat *Q = new Mat(); Mat *T = new Mat(); Calibration calibration; Triangulate triangulation; if (true) { vector<Mat> PandP; string filename = "stereo.xml"; PandP = calibration.Calibrate(filename, 5, 4, T, Q); cam0P = PandP.at(0); cam1P = PandP.at(1); } VideoCapture cap("/home/artyom/Dropbox/BigData/workspace/TriangulateHuman/Debug/out.avi"); /* cap.set(CV_CAP_PROP_FRAME_WIDTH, 320); cap.set(CV_CAP_PROP_FRAME_HEIGHT, 240); */ if (!cap.isOpened()) return -1; Mat img; VideoCapture cap1("/home/artyom/Dropbox/BigData/workspace/TriangulateHuman/Debug/out1.avi"); //cap.set(CV_CAP_PROP_FRAME_WIDTH, 320); //cap.set(CV_CAP_PROP_FRAME_HEIGHT, 240); if (!cap1.isOpened()) return -1; Mat img1; string Pos = ""; HOGDescriptor hog; hog.setSVMDetector(HOGDescriptor::getDefaultPeopleDetector()); //string posPoint = ""; string posRect =""; string posRect2 =""; bool onceOnly = true; while (true) { cap >> img; cap1 >> img1; /* Mat tempFrame = img; Mat tempFrame1 = img1; cv::transpose(img, tempFrame); cv::flip(tempFrame, tempFrame, 0); cv::transpose(tempFrame, img); cv::flip(img, img, 0); cv::transpose(img1, tempFrame1); cv::flip(tempFrame1, tempFrame1, 0); cv::transpose(tempFrame1, img1); cv::flip(img1, img1, 0); */ if (!img.data) continue; vector<Rect> found, found_filtered; vector<Rect> found1, found_filtered1; hog.detectMultiScale(img, found, 0, Size(8,8), Size(32,32), 1.05, 2); hog.detectMultiScale(img1, found1, 0, Size(8,8), Size(32,32), 1.05, 2); //hog.detect(img, found1, 0, Size(8,8), Size(0,0)); size_t i, j; for (i=0; i<found.size(); i++) { Rect r = found[i]; for (j=0; j<found.size(); j++) if (j!=i && (r & found[j])==r) break; if (j==found.size()) found_filtered.push_back(r); } for (i=0; i<found_filtered.size(); i++) { Rect r = found_filtered[i]; if (r.x > 0 && r.x < 500){ r.x += cvRound(r.width*0.1); r.width = cvRound(r.width*0.8); r.y += cvRound(r.height*0.06); r.height = cvRound(r.height*0.9); leftX = r.x; leftY = r.y; string x = to_string(r.x); string y = to_string(r.y); posRect = to_string(global) + " Pos: x:" + x+ " y: " + y; rectangle(img, r.tl(), r.br(), cv::Scalar(0,255,0), 2); } } for (i=0; i<found1.size(); i++) { Rect r = found1[i]; for (j=0; j<found1.size(); j++) if (j!=i && (r & found1[j])==r) break; if (j==found1.size()) found_filtered1.push_back(r); } for (i=0; i<found_filtered1.size(); i++) { Rect r = found_filtered1[i]; if (r.x > 0 && r.x < 500){ r.x += cvRound(r.width*0.1); r.width = cvRound(r.width*0.8); r.y += cvRound(r.height*0.06); r.height = cvRound(r.height*0.9); rightX = r.x; rightY = r.y; string x = to_string(r.x); string y = to_string(r.y); posRect2 = to_string(global) + " Pos: x:" + x+ " y: " + y; global++; rectangle(img1, r.tl(), r.br(), cv::Scalar(0,255,0), 2); } } int number = 5; char text[255]; sprintf(text, "Score %d", (int)number); CvFont font; double hScale=1.0; double vScale=1.0; int lineWidth=1; cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, hScale,vScale,0,lineWidth); IplImage* img00 = new IplImage(img); IplImage* img11 = new IplImage(img1); char* p = new char[posRect.length()+1]; memcpy(p, posRect.c_str(), posRect.length()+1); cvPutText(img00, p, cvPoint(200,400), &font, cvScalar(0,255,0)); char* p2 = new char[posRect2.length()+1]; memcpy(p2, posRect2.c_str(), posRect2.length()+1); cvPutText(img11, p2, cvPoint(200,300), &font, cvScalar(255,255,255)); double realXYZ[3]; triangulation.triangulate(cam0P, cam1P, leftX, leftY, rightX, rightY, *T, *Q, realXYZ); /*Debuggining purpose */ //printToFile(leftX, leftY,rightX, rightY, realXYZ.at(0)[0], realXYZ.at(0)[1], realXYZ.at(0)[2] ); printToFile(leftX, leftY,rightX, rightY, realXYZ[0], realXYZ[1], realXYZ[2]); imshow("video capture", img); imshow("video capture2", img1); if (waitKey(1) >= 0) break; } namedWindow("video capture", CV_WINDOW_AUTOSIZE); cout << "!!!Hello World!!!" << endl; // prints !!!Hello World!!! return 0; }
/// Render calibration void Tuning::renderCalibration(Calibration& _calib) const { _calib.render(*this,session_.exportSettings().outputMode()); }
Filter::Filter(const Calibration &calibration) : calibration_(calibration), accel_buffer_(ImuDevice::ACCELEROMETER, calibration.getBufferSize()), gyro_buffer_(ImuDevice::GYROSCOPE, calibration.getBufferSize()) { }
int main(int argc, char* argv[]) { if(argc < 4) { cerr << "Not enought parameters!" << endl; cerr << "Usage: " << argv[0] << " [calibration] [input] [output]" << endl; return EXIT_FAILURE; } // Loading calibration information cout << "Loading calibration data: " << argv[1] << "..."; Calibration calibration = Calibration(argv[1]); cout << endl; // Loading bundle cout << "Loading data: " << argv[2] << "..."; Bundle2 bundle(argv[2]); cout << endl; assert(calibration.num_cameras() == bundle.numCameras()); // Creating output file ofstream out(argv[3]); out << setprecision(6); // Writing header out << bundle.framesCount() << endl; /* out << "NbPoint2DMaxPerView 5000" << endl; */ /* out << "MaxAngularError 0.003578836342452" << endl; */ /* out << "MaxAngularError 0.007546901056936" << endl; */ out << endl; // Creating first triplets cout << endl; cout << "Exporting frame 0..."; vector<ShudaTriplet3D>* prevTriplets = new vector<ShudaTriplet3D>(); createFirstTriplets(bundle, calibration, *prevTriplets); writeTriplets(out, *prevTriplets); cout << "\r"; // Main loop vector<ShudaTriplet3D>* curTriplets = new vector<ShudaTriplet3D>(); for(size_t f = 1; f < bundle.framesCount() - 2; ++f) { cout << "Exporting frame " << f << "..."; createTriplets(bundle, calibration, f, *prevTriplets, *curTriplets); writeTriplets(out, *curTriplets); vector<ShudaTriplet3D>* tmp = prevTriplets; prevTriplets = curTriplets; curTriplets = tmp; cout << "\r"; cout.flush(); } cout << endl; // Clean up! delete curTriplets; delete prevTriplets; out.close(); return EXIT_SUCCESS; }
void MotionCore::initMemory() { // create all the modules that are used //MemorySource mem_source = MEMORY_SIM; //if (type_ == CORE_ROBOT) //mem_source = MEMORY_ROBOT; //Add required memory blocks memory_.addBlockByName("body_model"); memory_.addBlockByName("graphable"); memory_.addBlockByName("kick_engine"); memory_.addBlockByName("walk_engine"); memory_.addBlockByName("odometry"); //memory_.addBlockByName("game_state"); //memory_.addBlockByName("sonar"); memory_.getOrAddBlockByName(frame_info_,"frame_info"); // joint angles memory_.getBlockByName(raw_joint_angles_,"raw_joint_angles"); memory_.getOrAddBlockByName(processed_joint_angles_,"processed_joint_angles"); // commands memory_.getBlockByName(raw_joint_commands_,"raw_joint_commands"); memory_.getOrAddBlockByName(processed_joint_commands_,"processed_joint_commands"); // sensors memory_.getBlockByName(raw_sensors_,"raw_sensors"); memory_.getOrAddBlockByName(processed_sensors_,"processed_sensors"); memory_.getOrAddBlockByName(body_model_,"body_model"); memory_.getOrAddBlockByName(walk_request_,"walk_request"); memory_.getOrAddBlockByName(kick_params_,"kick_params"); memory_.getOrAddBlockByName(walk_param_,"walk_param"); memory_.getOrAddBlockByName(al_walk_param_,"al_walk_param"); memory_.getOrAddBlockByName(walk_info_,"walk_info"); memory_.getOrAddBlockByName(kick_request_,"kick_request"); memory_.getOrAddBlockByName(odometry_,"odometry"); memory_.getOrAddBlockByName(processed_sonar_,"processed_sonar"); memory_.getOrAddBlockByName(robot_info_,"robot_info",MemoryOwner::SHARED); // synchronized blocks - the true means remove any existing locks memory_.getOrAddBlockByName(sync_body_model_,"sync_body_model",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_joint_angles_,"sync_joint_angles",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_kick_request_,"sync_kick_request",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_odometry_,"sync_odometry",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_sensors_,"sync_sensors",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_walk_request_,"sync_walk_request",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_joint_commands_,"sync_joint_commands",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_processed_sonar_,"sync_processed_sonar",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_kick_params_,"sync_kick_params",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_walk_param_,"sync_walk_param",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_al_walk_param_,"sync_al_walk_param",MemoryOwner::SYNC); memory_.getOrAddBlockByName(sync_walk_info_,"sync_walk_info",MemoryOwner::SYNC); // Read configuration file and place appropriate values in memory if (type_ == CORE_ROBOT) { Calibration calibration; std::cout << "Reading calibration file: " << memory_.data_path_ + "calibration.txt" << std::endl; if (calibration.readFromFile(memory_.data_path_ + "calibration.txt")) { // Disabled these because of new calibration system - JM 05/08/13 //robot_info_->dimensions_.setCameraParameters(DEG_T_RAD*calibration.tilt_bottom_cam_, DEG_T_RAD*calibration.roll_bottom_cam_, DEG_T_RAD*calibration.tilt_top_cam_, DEG_T_RAD*calibration.roll_top_cam_); //robot_info_->dimensions_.setHeadOffsets(DEG_T_RAD * calibration.head_tilt_offset_, DEG_T_RAD * calibration.head_pan_offset_); } } // print out all the memory blocks we're using std::vector<std::string> memory_block_names; memory_.getBlockNames(memory_block_names,false); std::cout << "INITIAL MEMORY BLOCKS:" << std::endl; for (unsigned int i = 0; i < memory_block_names.size(); i++) std::cout << memory_block_names[i] << std::endl; std::cout << "--------------" << std::endl; textlog_.setFrameInfo(frame_info_); }
PointDescrModel::PointDescrModel(DXFReader* dxf, Signature* sig) : Descriptor(((ShapeModel*)sig->GetElement(0, DESCRIPTOR_SHAPE))->GetClass()), m_DescriptoHandel(-1), m_Config(new XMLTag(XML_NODE_POINTDESCRCONFIG)) { /* Load necessary information from the signature*/ ShapeModel* sm = (ShapeModel*)sig->GetElement(0, DESCRIPTOR_SHAPE); if(sm == NULL) throw "No shape model available"; Image* img = sm->GetLastMatchedImage(); if(img == NULL) throw "No matched image available"; ShapeModelParamSet* pm = sm->GetParamSet(); Calibration* calib = pm->m_calib; calib->SaveTo(m_Config); if(calib == NULL) throw "No Calibration found"; if(dxf == NULL) throw "No DXF-information available"; /* Create the overlapping region, where the 2d points have to be searched*/ try { #ifdef HALCONIMG if(sig->GetObjectPose() == NULL) throw "No Pose Available"; Halcon::Hobject xld = sm->GetContour(*sig->GetObjectPose()); try { m_objectPose = RelPoseFactory::CloneRelPose(sig->GetObjectPose()); } catch(...) { printf("Tying to copy a singular position\n"); m_objectPose = RelPoseFactory::FRelPoseWorld(); } int num = 0; Halcon::count_obj(xld, (Hlong*)&num); Halcon::Hobject region; /* Generate region for descriptor extraction*/ Halcon::gen_empty_region(®ion); for(int i = 0; i < num; i++) { Halcon::Hobject obj; Halcon::gen_region_contour_xld(xld, &obj ,"filled"); Halcon::union2(obj, region, ®ion); } Halcon::connection(region, ®ion); Halcon::fill_up(region, ®ion); Halcon::Hobject reducedimg; Halcon::reduce_domain(*img->GetHImage(), region, &reducedimg); /* Get Camparam */ Halcon::HTuple camparam = calib->CamParam(); /* Prepare 3d and 2d point in htuples */ std::vector<face3dTransformed> vec = dxf->m_3dFaceData; Halcon::HTuple ox, oy, oz; Halcon::HTuple r, c; Halcon::Hobject reg; Halcon::gen_empty_region(®); /* Visibility Test for all points...*/ for(std::vector<face3dTransformed>::const_iterator iter = vec.begin(); iter != vec.end(); iter++) { Halcon::Hobject obj; Halcon::HTuple x, y, z; Halcon::HTuple rpoly, cpoly, test; x.Append((*iter).m_x1); y.Append((*iter).m_y1); z.Append((*iter).m_z1); x.Append((*iter).m_x2); y.Append((*iter).m_y2); z.Append((*iter).m_z2); x.Append((*iter).m_x3); y.Append((*iter).m_y3); z.Append((*iter).m_z3); x.Append((*iter).m_x4); y.Append((*iter).m_y4); z.Append((*iter).m_z4); Halcon::project_3d_point(x,y,z, camparam, &rpoly,&cpoly); Halcon::HTuple rb, cb; rb = rpoly[0].D(); cb = cpoly[0].D(); rpoly.Append(rb); cpoly.Append(cb); Halcon::test_region_point(reg, rb,cb, &test); bool b = test[0].I() == 0; rb = rpoly[1].D(); cb = cpoly[1].D(); Halcon::test_region_point(reg, rb,cb, &test); b = b || test[0].I() == 0; rb = rpoly[2].D(); cb = cpoly[2].D(); Halcon::test_region_point(reg, rb,cb, &test); b = b || test[0].I() == 0; rb = rpoly[3].D(); cb = cpoly[3].D(); Halcon::test_region_point(reg, rb,cb, &test); b = b || test[0].I() == 0; if(b) { ox.Append((*iter).m_ox1); oy.Append((*iter).m_oy1); oz.Append((*iter).m_oz1); r.Append(rpoly[0].D()); c.Append(cpoly[0].D()); ox.Append((*iter).m_ox2); oy.Append((*iter).m_oy2); oz.Append((*iter).m_oz2); r.Append(rpoly[1].D()); c.Append(cpoly[1].D()); ox.Append((*iter).m_ox3); oy.Append((*iter).m_oy3); oz.Append((*iter).m_oz3); r.Append(rpoly[2].D()); c.Append(cpoly[2].D()); ox.Append((*iter).m_ox3); oy.Append((*iter).m_oy3); oz.Append((*iter).m_oz3); r.Append(rpoly[2].D()); c.Append(cpoly[2].D()); ox.Append((*iter).m_ox4); oy.Append((*iter).m_oy4); oz.Append((*iter).m_oz4); r.Append(rpoly[3].D()); c.Append(cpoly[3].D()); ox.Append((*iter).m_ox1); oy.Append((*iter).m_oy1); oz.Append((*iter).m_oz1); r.Append(rpoly[0].D()); c.Append(cpoly[0].D()); } Halcon::gen_region_polygon_filled(&obj, rpoly, cpoly); Halcon::union2(reg, obj, ®); } /* Get the 2d points*/ //Halcon::project_3d_point(x,y,z, camparam, &r,&c); Halcon::HTuple modelID, empty, hommat; m_objectPose->GetHommat(&hommat, NULL); /* Create the descriptor model */ Halcon::create_descriptor_model_3d_calib(reducedimg, "harris", empty,empty, "randomized_fern", empty, empty, 42,ox,oy,oz,r,c, camparam, hommat ,&modelID); m_DescriptoHandel = modelID[0].L(); printf("Write Descriptor Model to File\n"); std::string stFileName("descr_obj.dcm"); Halcon::write_descriptor_model_3d(modelID, stFileName.c_str()); m_Config->AddProperty(XML_ATTRIBUTE_POINTDESCRFILE, stFileName); } catch(Halcon::HException ex) { printf("Error while Learning: %s\n", ex.message); throw "Error in Point DescrModel"; #endif } catch(...) { printf("Error while Learning: ---- unknown error ---\n"); throw "Error in Point DescrModel"; } delete calib; }
int main( int argc, char ** argv ) { char * in_file = NULL; char * out_file = NULL; unsigned int max_it = 2000; // Max nr. of iterations bool print_input = false; bool verbose = false; int mode = MODE_PT; // 1: PocketTopo Optimize // 2: Optimize2 // 3: OptimizeBest // 4: PocketTopo Optimize with optimize_axis // 5: OptimizeBest with optimize_axis // 6: Optimize M at fixed G // 7: Optimize iterative double delta = 0.5; // required delta for OptimizeBest double error; unsigned int iter = 0; int ac = 1; while ( ac < argc ) { if ( strncmp(argv[ac],"-i",2) == 0 ) { max_it = atoi( argv[ac+1]); if ( max_it < 200 ) max_it = 200; ac += 2; #ifdef EXPERIMENTAL } else if ( strncmp(argv[ac],"-m", 2) == 0 ) { mode = atoi( argv[ac+1] ); if ( mode < MODE_PT || mode >= MODE_MAX ) mode = MODE_PT; ac += 2; } else if ( strncmp(argv[ac],"-d", 2) == 0 ) { delta = atof( argv[ac+1] ); if ( delta < 0.001 ) delta = 0.5; ac += 2; #endif } else if ( strncmp(argv[ac],"-p", 2) == 0 ) { print_input = true; ac ++; } else if ( strncmp(argv[ac],"-v", 2) == 0 ) { verbose = true; ac ++; } else if ( strncmp(argv[ac],"-h", 2) == 0 ) { usage(); ac ++; } else { break; } } if ( ac >= argc ) { usage(); return 0; } in_file = argv[ac]; ac ++; if ( argc > ac ) { out_file = argv[ac]; } if ( verbose ) { fprintf(stderr, "Calibration data file \"%s\"\n", in_file ); if ( out_file ) { fprintf(stderr, "Calibration coeff file \"%s\"\n", out_file ); } fprintf(stderr, "Max nr. iterations %d \n", max_it ); #ifdef EXPERIMENTAL fprintf(stderr, "Optimization mode: nr. %d\n", mode ); #endif } FILE * fp = fopen( in_file, "r" ); if ( fp == NULL ) { fprintf(stderr, "ERROR: cannot open input file \"%s\"\n", in_file); return 0; } Calibration calib; int16_t gx, gy, gz, mx, my, mz; int grp; int ignore; int cnt = 0; char line[256]; if ( fgets(line, 256, fp) == NULL ) { // empty file fclose( fp ); return false; } if ( line[0] == '0' && line[1] == 'x' ) { // calib-coeff format if ( verbose ) { fprintf(stderr, "Input file format: calib-coeff\n"); } int gx0, gy0, gz0, mx0, my0, mz0; // skip coeffs; for (int k=1; k<6; ++k ) fgets(line, 256, fp); // skip one more line fgets(line, 256, fp); // printf("reading after: %s\n", line); while ( fgets(line, 255, fp ) ) { // fprintf(stderr, line ); char rem[32]; if ( line[0] == '#' ) continue; sscanf( line, "G: %d %d %d M: %d %d %d %s %d %d", &gx0, &gy0, &gz0, &mx0, &my0, &mz0, rem, &grp, &ignore); gx = (int16_t)( gx0 ); gy = (int16_t)( gy0 ); gz = (int16_t)( gz0 ); mx = (int16_t)( mx0 ); my = (int16_t)( my0 ); mz = (int16_t)( mz0 ); calib.AddValues( gx, gy, gz, mx, my, mz, cnt, grp, ignore ); ++cnt; if ( verbose ) { fprintf(stderr, "%d G: %d %d %d M: %d %d %d [%d]\n", cnt, gx, gy, gz, mx, my, mz, grp ); } } } else { if ( verbose ) { fprintf(stderr, "Input file format: calib-data\n"); } rewind( fp ); unsigned int gx0, gy0, gz0, mx0, my0, mz0; while ( fgets(line, 255, fp ) ) { // fprintf(stderr, line ); if ( line[0] == '#' ) continue; sscanf( line, "%x %x %x %x %x %x %d %d", &gx0, &gy0, &gz0, &mx0, &my0, &mz0, &grp, &ignore); gx = (int16_t)( gx0 & 0xffff ); gy = (int16_t)( gy0 & 0xffff ); gz = (int16_t)( gz0 & 0xffff ); mx = (int16_t)( mx0 & 0xffff ); my = (int16_t)( my0 & 0xffff ); mz = (int16_t)( mz0 & 0xffff ); calib.AddValues( gx, gy, gz, mx, my, mz, cnt, grp, ignore ); ++cnt; if ( verbose ) { fprintf(stderr, "%d G: %d %d %d M: %d %d %d [%d]\n", cnt, gx, gy, gz, mx, my, mz, grp ); } } } fclose( fp ); if ( print_input ) { calib.PrintValues(); // { int i; scanf("%d", &i); } // calib.CheckInput(); // { int i; scanf("%d", &i); } calib.PrintGroups(); } calib.PrepareOptimize(); switch ( mode ) { case MODE_PT: iter = calib.Optimize( delta, error, max_it ); break; #ifdef EXPERIMENTAL case MODE_GRAD: iter = calib.Optimize2( delta, error, max_it ); break; case MODE_DELTA: iter = calib.OptimizeBest( delta, error, max_it ); break; case MODE_FIX_G: { // TODO set the G coeffs Vector b( 0.0037, -0.0725, -0.0300); Vector ax( 1.7678, -0.0013, 0.0119); Vector ay( -0.0023, 1.7657, -0.0016); Vector az(-0.0112, -0.0016, 1.7660); Matrix a( ax, ay, az ); iter = calib.Optimize( delta, error, max_it ); fprintf(stdout, "Iterations %d\n", iter); fprintf(stdout, "Delta %.4f\n", delta ); fprintf(stdout, "Max error %.4f\n", error ); calib.PrintCoeffs(); delta = 0.5; iter = 0; error = 0.0; calib.SetGCoeffs( a, b ); iter = calib.OptimizeM( delta, error, max_it ); } break; case MODE_ITER: /* Vector b( 0.0037, -0.0725, -0.0300); Vector ax( 1.7678, -0.0013, 0.0119); Vector ay( -0.0023, 1.7657, -0.0016); Vector az(-0.0112, -0.0016, 1.7660); Matrix a( ax, ay, az ); calib.SetGCoeffs( a, b ); */ iter = calib.OptimizeIterative( delta, error, max_it ); break; #ifdef USE_GUI case MODE_PT_DISPLAY: calib.SetShowGui( true ); iter = calib.Optimize( delta, error, max_it ); // OptimizeExp( delta, error, max_it, true ); break; case MODE_DELTA_DISPLAY: calib.SetShowGui( true ); iter = calib.OptimizeBest( delta, error, max_it, true ); break; #endif #endif // EXPERIMENTAL default: fprintf(stderr, "Unexpected calibration mode %d\n", mode ); break; } fprintf(stdout, "Iterations %d\n", iter); fprintf(stdout, "Delta %.4f\n", delta ); fprintf(stdout, "Max error %.4f\n", error ); fprintf(stderr, "M dip angle %.2f\n", calib.GetDipAngle() ); if ( verbose ) { calib.PrintCoeffs(); // calib.CheckInput(); } #if 0 unsigned char data[48]; calib.GetCoeff( data ); for (int k=0; k<48; ++k) { printf("0x%02x ", data[k] ); if ( ( k % 4 ) == 3 ) printf("\n"); } #endif if ( out_file != NULL ) { calib.PrintCalibrationFile( out_file ); } return 0; }