Eigen最小二乘法拟合三次曲线

用osg做了可视化验证,交互点都存在v3d中了，用了三次曲线作为曲线方程，即y= ax3 + bx2 + cx + d,以一组观测值x,y序列去求未知数a,b,c,d，当然观测个数必须大于4

#include <osgViewer/Viewer>
#include <osgDB/ReadFile>
#include <osgGA/GUIEventHandler>
#include <osg/ShapeDrawable>
#include "OrthoManipulator.h"
#include <Eigen/Dense>


using namespace std;
using namespace Eigen;


osg::ref_ptr<osg::Group> root = new osg::Group;
osg::ref_ptr<osg::Vec3dArray> v3d = new osg::Vec3dArray;


osg::Matrixd getVPW(osgViewer::View* view)
{
osg::Matrix VPW = view->getCamera()->getViewMatrix()*
view->getCamera()->getProjectionMatrix()*
view->getCamera()->getViewport()->computeWindowMatrix();
return VPW;
}


osg::Vec3d pick(osgViewer::View* view, int winx, int winy)
{
osg::Vec3d result;
result = osg::Vec3d(winx, winy, 0.1) * osg::Matrix::inverse(getVPW(view));
result.z() = 0.0;


return result;
}


class MyGUIHandler : public osgGA::GUIEventHandler
{
public:
bool handle(const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& aa)
{
if(ea.getHandled()) return false;


osgViewer::Viewer* viewer = dynamic_cast<osgViewer::Viewer*>(&aa);
if (!viewer) return false;


if(ea.getEventType() == osgGA::GUIEventAdapter::RELEASE && ea.getButton() == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON)
{
osg::Vec3d pos = pick(viewer, ea.getX(), ea.getY());
v3d->push_back(pos);

osg::Geode* geode = new osg::Geode;
geode->addDrawable(new osg::ShapeDrawable(new osg::Sphere(pos, 1.0f)));
root->addChild(geode);
}
if(ea.getEventType() == osgGA::GUIEventAdapter::KEYUP && ea.getKey() == osgGA::GUIEventAdapter::KEY_Space)
{
int rows = v3d->size();
MatrixXf A(rows, 4);
for(int i = 0 ; i < rows; i++)
{
double x = v3d->at(i).x();
A(i, 0) = x * x * x;
A(i, 1) = x * x;
A(i, 2) = x;
A(i, 3) = 1;
}


VectorXf b(rows);
for(int i = 0 ; i < rows; i++)
{
double y = v3d->at(i).y();
b(i) = y;
}


VectorXf d =  A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);


cout << "The least-squares solution is:\n"
<< d << endl;


osg::Vec3dArray* vv3 = new osg::Vec3dArray;
for(int i = 0; i < rows; i++)
{
double x = v3d->at(i).x();
VectorXf b(4);
b(0) = x * x * x;
b(1) = x * x;
b(2) = x;
b(3) = 1;
double y = b.transpose() * d;


vv3->push_back(osg::Vec3d(x, y, 0));
}


osg::Geometry* geom = new osg::Geometry;
geom->setVertexArray(vv3);
geom->addPrimitiveSet(new osg::DrawArrays(osg::DrawArrays::LINE_STRIP, 0, vv3->size()));


osg::Geode* geode = new osg::Geode;
geode->addDrawable(geom);
root->addChild(geode);


}


return false;
}


};


int main()
{
osg::ref_ptr<osgViewer::Viewer> viewer = new osgViewer::Viewer;
viewer->addEventHandler(new MyGUIHandler);
viewer->setCameraManipulator(new OrthoManipulator(viewer));
viewer->setSceneData(root);
return viewer->run();
}