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;
}
