int main__(int argc, char * argv[])
{
// Create the root node Group.
osg::ref_ptr<osg::Group> root = new osg::Group;
// Read the object
osg::ref_ptr<osg::Node> cow = osgDB::readNodeFile( "D:/Programs64/OSG/Data/OpenSceneGraph-Data-3.0.0/cow.osg" );
// Define a matrix transform
osg::ref_ptr<osg::MatrixTransform> mt = new osg::MatrixTransform;
// Set scene graph
mt->addChild(cow);
root->addChild(mt);
// Create animation path
osg::ref_ptr<osg::AnimationPath> path = new osg::AnimationPath;
// Define control points
osg::AnimationPath::ControlPoint CP0(osg::Vec3( 2.f, -1.f, 0.f ));
osg::AnimationPath::ControlPoint CP1(osg::Vec3( 2.f, 1.f, 0.f )//,osg::Quat(-osg::PI/6,osg::Vec3( 0.f, 0.f, 1.f ))
);
//osg::AnimationPath::ControlPoint CP2 (osg::Vec3( -2.f, -1.f, 0.f ),osg::Quat(-osg::PI*2/6,osg::Vec3( 0.f, 0.f, 1.f )),osg::Vec3( 0.5f, 0.5f, 0.5f ));
//osg::AnimationPath::ControlPoint CP3 (osg::Vec3( 2.f, -1.f, 0.f ),osg::Quat(-osg::PI*3/6,osg::Vec3( 0.f, 0.f, 1.f )));
//osg::AnimationPath::ControlPoint CP4 (osg::Vec3( 2.f, 1.f, 0.f ));
// Insert them to the path
path->insert( 0.0f, CP0 ); // time, point
path->insert( 1.0f, CP1 );
//path->insert( 2.0f, CP2 );
//path->insert( 3.0f, CP3 );
//path->insert( 4.0f, CP4 );
//path->setLoopMode(osg::AnimationPath::LoopMode::SWING);
// Define animation path callback
osg::ref_ptr<osg::AnimationPathCallback> APCallback = new osg::AnimationPathCallback(path.get() );
// Update the matrix transform
mt->setUpdateCallback( APCallback.get() );
// create the view of the scene.
osgViewer::Viewer viewer;
viewer.setSceneData( root.get() );
return( viewer.run() );
}
void test_EKF_remove_feature_landmark_noise_ptz_data()
{
vcl_string ptz_file("/Users/jimmy/Desktop/images/33_slam_data/ptz_145420_145719.txt");
vcl_vector<PTZData> ptzs;
bool isRead = readPTZCameraFile(ptz_file.c_str(), ptzs, 1);
assert(isRead);
vcl_vector<double> gdPans;
vcl_vector<double> gdTilts;
vcl_vector<double> gdZooms;
vcl_vector<double> observedPans;
vcl_vector<double> observedTilts;
vcl_vector<double> observedZooms;
vcl_vector<vcl_vector<vgl_point_2d<double> > > observedImagePts;
const int width = 1280;
const int height = 720;
vnl_random rnd;
double delta = 2.0;
vgl_point_2d<double> pp(width/2, height/2);
PTZKeypointDynamicEKF ptzDynamicEKF;
// const int M = (int)firstFeatures.size();
vnl_vector<double> C0(6, 0);
C0[0] = ptzs[0].pan;
C0[1] = ptzs[0].tilt;
C0[2] = ptzs[0].fl;
vnl_matrix<double> CP0(6, 6, 0);
CP0(0, 0) = 0.01; CP0(1, 1) = 0.01; CP0(2, 2) = 10;
CP0(3, 3) = 0.001; CP0(4, 4) = 0.001; CP0(5, 5) = 0.1;
vnl_matrix<double> CQ0(6, 6, 0);
CQ0(0, 0) = 0.00000004; CQ0(1, 1) = 0.00000004; CQ0(2, 2) = 0.0000004;
CQ0(3, 3) = 0.00000001; CQ0(4, 4) = 0.00000001; CQ0(5, 5) = 0.0000001;
// construct feature a database
vcl_vector<vgl_point_2d<double> > courtPoints = DisneyWorldBasketballCourt::getCalibPoints();
vcl_list<VxlEKFFeaturePoint> featureDatabase;
for (int i = 0; i<courtPoints.size(); i++) {
vgl_point_3d<double> p(courtPoints[i].x(), courtPoints[i].y(), 0);
vgl_point_2d<double> q(0, 0);
VxlEKFFeaturePoint feat(p, q);
feat.id_ = i;
featureDatabase.push_back(feat);
}
// init camera and feature from ground truth of data set
VxlEKFCamera camera(C0, CP0, CQ0);
vcl_list<VxlEKFFeaturePoint> features;
for (int i = 0; i<courtPoints.size(); i++) {
vgl_homg_point_3d<double> p(courtPoints[i].x(), courtPoints[i].y(), 0, 1.0);
if (camera.is_behind_camera(p)) {
continue;
}
vgl_point_2d<double> q= camera.project(p);
if (vgl_inside_image(q, width, height, 10)) {
VxlEKFFeaturePoint feat(p, q);
feat.id_ = i;
features.push_back(feat);
}
}
printf("initiate feature number is %lu\n", features.size());
vnl_vector<double> Xk;
vnl_matrix<double> Pk;
bool isInit = ptzDynamicEKF.updateCameraFeature(camera, features, Xk, Pk);
assert(isInit);
vcl_vector<double> smoothedPans;
vcl_vector<double> smoothedTilts;
vcl_vector<double> smoothedZooms;
for (int i = 1; i<ptzs.size(); i++) { // ptzs.size()
gdPans.push_back(ptzs[i].pan);
gdTilts.push_back(ptzs[i].tilt);
gdZooms.push_back(ptzs[i].fl);
vpgl_perspective_camera<double> gdCamera;
bool isCamera = VxlPTZCamera::PTZToCamera(ptzs[i].fl, ptzs[i].pan, ptzs[i].tilt, gdCamera);
assert(isCamera);
// remove features
vcl_list<VxlEKFFeaturePoint>::iterator it = features.begin();
while (it != features.end()) {
vgl_point_3d<double> p = it->worldPt();
if (gdCamera.is_behind_camera(vgl_homg_point_3d<double>(p.x(), p.y(), p.z(), 1.0)))
{
it = features.erase(it);
printf("erase a feature\n");
continue;
}
vgl_point_2d<double> q = gdCamera.project(p);
if (!vgl_inside_image(q, width, height, 10)) {
it = features.erase(it);
printf("erase a feature\n");
continue;
}
it++;
}
printf("feature number is %lu\n", features.size());
// add features
for (vcl_list<VxlEKFFeaturePoint>::iterator it = featureDatabase.begin(); it != featureDatabase.end(); it++) {
vgl_point_3d<double> p = it->worldPt();
if (gdCamera.is_behind_camera(vgl_homg_point_3d<double>(p.x(), p.y(), p.z(), 1.0)))
{
continue;
}
vgl_point_2d<double> q = gdCamera.project(p);
if (vgl_inside_image(q, width, height, 10))
{
vcl_list<VxlEKFFeaturePoint>::iterator findIt = std::find(features.begin(), features.end(), *it);
// cannot find same featuere (defined by id) in the features
if (findIt == features.end()) {
VxlEKFFeaturePoint feat(p, q);
feat.id_ = it->id_;
features.push_back(feat);
printf("add a new feature with id %d\n", (int)feat.id_);
}
}
}
vcl_vector<vgl_point_2d<double> > worldPts;
vcl_vector<vgl_point_2d<double> > imagePts;
// add noise to current observation
for (vcl_list<VxlEKFFeaturePoint>::iterator it = features.begin(); it != features.end(); it++) {
vgl_point_3d<double> p = it->worldPt();
vgl_point_2d<double> q = gdCamera.project(p);
double x = q.x();
double y = q.y();
x += delta * rnd.normal();
y += delta * rnd.normal();
it->setImagePoint(x, y);
worldPts.push_back(vgl_point_2d<double>(p.x(), p.y()));
imagePts.push_back(it->imagePt());
}
vpgl_perspective_camera<double> initCamera;
vpgl_perspective_camera<double> finalCamera;
bool isInit = VpglPlus::init_calib(worldPts, imagePts, pp, initCamera);
if (!isInit) {
printf("initiate camera error\n");
continue;
}
bool isFinal = VpglPlus::optimize_perspective_camera(worldPts, imagePts, initCamera, finalCamera);
if (!isFinal) {
printf("final camera error\n");
continue;
}
//
double pan = 0;
double tilt = 0;
double zoom = 0;
bool isPTZ = VxlPTZCamera::CameraToPTZ(finalCamera, pan, tilt, zoom);
assert(isPTZ);
observedPans.push_back(pan);
observedTilts.push_back(tilt);
observedZooms.push_back(zoom);
printf("observed pan tilt focal length is %f %f %f\n", pan, tilt, zoom);
ptzDynamicEKF.updateCameraFeature(camera, features, Xk, Pk);
printf("gd pan tilt focal length is %f %f %f\n\n", ptzs[i].pan, ptzs[i].tilt, ptzs[i].fl);
smoothedPans.push_back(Xk[0]);
smoothedTilts.push_back(Xk[1]);
smoothedZooms.push_back(Xk[2]);
}
//save
vnl_vector<double> gdPanVec(&gdPans[0], (int)gdPans.size());
vnl_vector<double> observedPanVec(&observedPans[0], (int)observedPans.size());
vnl_vector<double> smoothedPanVec(&smoothedPans[0], (int)smoothedPans.size());
vnl_vector<double> gdZoomVec(&gdZooms[0], (int)gdZooms.size());
vnl_vector<double> observedZoomVec(&observedZooms[0], (int)observedZooms.size());
vnl_vector<double> smoothedZoomVec(&smoothedZooms[0], (int)smoothedZooms.size());
vnl_vector<double> gdTiltVec(&gdTilts[0], (int)gdTilts.size());
vnl_vector<double> observedTiltVec(&observedTilts[0], (int)observedTilts.size());
vnl_vector<double> smoothedTiltVec(&smoothedTilts[0], (int)observedTilts.size());
vcl_string save_file("EKF_dynamic_ptz.mat");
vnl_matlab_filewrite awriter(save_file.c_str());
awriter.write(gdPanVec, "gdPan");
awriter.write(observedPanVec, "observedPan");
awriter.write(smoothedPanVec, "sm_pan");
awriter.write(gdZoomVec, "gdZoom");
awriter.write(observedZoomVec, "observedZoom");
awriter.write(smoothedZoomVec, "smoothedZoom");
awriter.write(gdTiltVec, "gdTilt");
awriter.write(vnl_vector<double>(&observedTilts[0], (int)observedTilts.size()), "observedTilt");
awriter.write(vnl_vector<double>(&smoothedTilts[0], (int)observedTilts.size()), "smoothedTilt");
printf("save to %s\n", save_file.c_str());
}
int main(int argc, char** argv)
{
osg::ArgumentParser arguments(&argc, argv);
arguments.getApplicationUsage()->setApplicationName(arguments.getApplicationName());
arguments.getApplicationUsage()->setDescription(arguments.getApplicationName()+" is an example for viewing osgAnimation animations.");
arguments.getApplicationUsage()->addCommandLineOption("-h or --help","List command line options.");
arguments.getApplicationUsage()->addCommandLineOption("--drawbone","draw helps to display bones.");
if (arguments.read("-h") || arguments.read("--help"))
{
arguments.getApplicationUsage()->write(std::cout, osg::ApplicationUsage::COMMAND_LINE_OPTION);
return 0;
}
//if (arguments.argc()<=1)
//{
// arguments.getApplicationUsage()->write(std::cout, osg::ApplicationUsage::COMMAND_LINE_OPTION);
// return 1;
//}
bool drawBone = true;
if (arguments.read("--drawbone"))
drawBone = true;
osgViewer::Viewer viewer(arguments);
osg::ref_ptr<osg::Group> group = new osg::Group();
osg::Group* node = dynamic_cast<osg::Group*>(osgDB::readNodeFile("C:/Matej/test_1.osg"));
//dynamic_cast<osg::Group*>(osgDB::readNodeFiles(arguments)); //dynamic_cast<osgAnimation::AnimationManager*>(osgDB::readNodeFile(psr[1]));
if(!node)
{
std::cout << arguments.getApplicationName() <<": No data loaded" << std::endl;
return 1;
}
cutSimulation(node);
// Set our Singleton's model.
AnimationManagerFinder finder;
node->accept(finder);
if (finder._am.valid()) {
node->setUpdateCallback(finder._am.get());
AnimtkViewerModelController::setModel(finder._am.get());
osgAnimation::BasicAnimationManager* model = finder._am.get();
const osgAnimation::AnimationList & lstAnimation = model->getAnimationList();
const osg::ref_ptr<osgAnimation::Animation> & canim = *(lstAnimation.begin());
osg::ref_ptr<osgAnimation::Animation> & anim = const_cast<osg::ref_ptr<osgAnimation::Animation> &>(canim);
} else {
osg::notify(osg::WARN) << "no osgAnimation::AnimationManagerBase found in the subgraph, no animations available" << std::endl;
}
if (drawBone) {
osg::notify(osg::INFO) << "Add Bones Helper" << std::endl;
AddHelperBone addHelper;
node->accept(addHelper);
}
node->addChild(createAxis());
AnimtkViewerGUI* gui = new AnimtkViewerGUI(&viewer, WIDTH, HEIGHT, 0x1234);
osg::Camera* camera = gui->createParentOrthoCamera();
node->setNodeMask(0x0001);
group->addChild(node);
group->addChild(camera);
viewer.addEventHandler(new AnimtkKeyEventHandler());
viewer.addEventHandler(new osgViewer::StatsHandler());
viewer.addEventHandler(new osgViewer::WindowSizeHandler());
viewer.addEventHandler(new osgGA::StateSetManipulator(viewer.getCamera()->getOrCreateStateSet()));
viewer.addEventHandler(new osgWidget::MouseHandler(gui));
viewer.addEventHandler(new osgWidget::KeyboardHandler(gui));
viewer.addEventHandler(new osgWidget::ResizeHandler(gui, camera));
//viewer.setSceneData(group.get());
viewer.setUpViewInWindow(40, 40, WIDTH, HEIGHT);
// Create the root node Group.
osg::ref_ptr<osg::Group> root = new osg::Group;
// Read the object
//osg::ref_ptr<osg::Node> cow = osgDB::readNodeFile( "D:/Programs64/OSG/Data/OpenSceneGraph-Data-3.0.0/cow.osg" );
// Define a matrix transform
osg::ref_ptr<osg::MatrixTransform> mt = new osg::MatrixTransform;
// Set scene graph
mt->addChild(group);
root->addChild(mt);
// Create animation path
osg::ref_ptr<osg::AnimationPath> path = new osg::AnimationPath;
// Define control points
osg::AnimationPath::ControlPoint CP0(osg::Vec3( 0.f, 5.f, 0.f ));
osg::AnimationPath::ControlPoint CP1(osg::Vec3( -2.5f, -2.5f, 0.f ),osg::Quat(-osg::PI/4,osg::Vec3( 0.f, 0.f, 1.f ))
);
//osg::AnimationPath::ControlPoint CP2 (osg::Vec3( -2.f, -1.f, 0.f ),osg::Quat(-osg::PI*2/6,osg::Vec3( 0.f, 0.f, 1.f )),osg::Vec3( 0.5f, 0.5f, 0.5f ));
//osg::AnimationPath::ControlPoint CP3 (osg::Vec3( 2.f, -1.f, 0.f ),osg::Quat(-osg::PI*3/6,osg::Vec3( 0.f, 0.f, 1.f )));
//osg::AnimationPath::ControlPoint CP4 (osg::Vec3( 2.f, 1.f, 0.f ));
// Insert them to the path
path->insert( 0.0f, CP0 ); // time, point
path->insert( 1.0f, CP1 );
//path->insert( 2.0f, CP2 );
//path->insert( 3.0f, CP3 );
//path->insert( 4.0f, CP4 );
//path->setLoopMode(osg::AnimationPath::LoopMode::SWING);
// Define animation path callback
osg::ref_ptr<osg::AnimationPathCallback> APCallback = new osg::AnimationPathCallback(path.get() );
// Update the matrix transform
mt->setUpdateCallback( APCallback.get() );
viewer.setSceneData(root.get());
return viewer.run();
}
