Levmar配置
原文有些錯誤，在我的博客裏已經改好了：http://blog.sina.com.cn/s/blog_45b747f70101he1t.html
如果6或者7自由度機器人的運動學逆解無法找到解析解，我們就要用數值解法來處理。這屬於非線性超越方程的數值解法問題。LM算法，也成爲阻尼最小二乘法。
本人編程能力太弱，只好藉助於庫函數。看到網上教程有用Levmar（FAQ）來做LM算法的。Levmar可用於C++, Matlab,Perl, Python, Haskell and Tcl。本人在VS2010下編程，所以下面只談談如何在VS2010下配置。
經過測試，win7與win8 64位系統下的VS2010/VS2013用下面的方法配置可以成功。
準備工具：
1. cmake-2.8.12.1-win32-x86.zip (選擇Binarydistributions欄下的第二個)。
2. clapack-3.2.1-CMAKE.tgz（頁面做的稍亂，找到同名的那個壓縮包下載）。
3. levmar-2.6
步驟：
1. 先將上面下載的三個壓縮包都解壓。解壓後的cmake-2.8.12.1-win32-x86是編譯工具，建議放在常用的工具文件夾下（英文目錄，不要有中文）。解壓後的clapack-3.2.1-CMAKE和levmar-2.6等會要用到，而且編程的時候也用到其做包含目錄，因此不建議放在桌面。
2. 針對clapack-3.2.1-CMAKE的操作。雙擊打開cmake-gui.exe（該文件在cmake-2.8.12.1-win32-x86文件夾裏的bin文件夾下）首先要對clapack-3.2.1-CMAKE進行編譯。如圖：
對於箭頭1，點擊右邊的BrowseSource...將文件夾定位到第一步解壓出的clapack3.2.1-CMAKE，對於箭頭2，Browse Build..則是你要編譯到的位置，爲了方便我選擇C:\CLAPACK。選擇完之後，我們點擊左下角的configure按鈕，彈出的框裏我們選擇Visual Studio 10 Win64，點Finish等幾秒鐘，Configure結束後畫面中間會出來一些紅色的條框，不管它，我們再點Configure右邊的Generate按鈕，根據下面提示Generate done後，可以先關掉這個窗口了。現在我們到C:\CLAPACK下，找到CLAPACK.sln這個文件，並雙擊，這時候會在VS2010裏出來一個工程，等待加載完成後，我們選擇生成解決方案。這裏需要花點時間，等程序跑完之後會提示成功了33個項目（這一步是爲了生成4個lib文件）。下面我們可以先關掉VS2010。然後再次到C:\CLAPACK文件夾下，我們要尋找4個.lib文件（爲了在levmar中要鏈接他們），這4個文件分別是：
l C:\Clapack\BLAS\SRC\Debug: blas.lib
l C:\Clapack\F2CLIBS\libf2c\Debug: libf2c.lib
l C:\Clapack\SRC\Debug: lapack.lib
l C:\Clapack\Testing\MATGEN\Debug: tmglib.lib
爲了便於我們在後面鏈接他們，我們先在C:\CLAPACK下新建一個LIB文件夾，然後複製上面四個lib文件到這個文件夾下。到這裏第二步基本完成了。
注意：複製libf2c.lib的時候我們將文件名改爲f2c.lib（因爲後面鏈接的默認名是f2c.lib）
3. 針對LEVMAR2.6部分接下去我們再次打開cmake-gui.exe,Browse Source...選擇第一步解壓出來的levmar-2.6，Browse Build..選擇要編譯到的位置，這裏我選的是C:\lm26。然後點擊Configure，選擇Visual Studio 10 Win64，之後彈出如下所示：
將LAPACKBLAS_DIR 後面的usr/lib文件夾路徑改成第二步中我們自己建的那個文件夾路徑 C:\CLAPACK\LIB。然後再次點擊左下角的Configure，紅色變白色，之後再點擊Generate。下面我們到 C:\lm26文件夾下找到LEVMAR.sln文件，運行它，點生成解決方案，全部成功後關掉。這一步是爲裏生成C:\lm26\Debug\levmar.lib文件。到這裏配置部分基本完成了，下面講如何在我們自己的程序裏用。
4. 新建一個VS2010的c++工程lm_test，點頂部菜單欄的項目，再點屬性，點左側的配置屬性，再點右側的配置管理器，點活動平臺解決方案的下拉菜單，點新建，然後把Itanium換成x64點確定，如圖
這樣，我們就將我們的工程配置成了64位的。
然後右鍵點擊左側解決方案資源管理器中lm_test，選擇屬性,彈出的窗口左側點擊配置屬性下的VC++目錄，然後在右側的包含目錄中添加我們在第一步中解壓出的levmar2.6的文件夾，如圖
確定後，我們再在庫目錄下再添加兩個文件夾,一個是我們在第二步中自己新建的那個C:\CLAPACK\LIB,另一個是C:\lm26\Debug,爲的就是這兩個文件夾裏面的lib文件，如圖:
確定後我們點擊左側的鏈接器、輸入，附加依賴項添加第二步生成4個lib文件和第三步生成的1個lib文件: lapack.lib, blas.lib, f2c.lib,tmglib.lib.levmar.lib。
現在把下面的程序拷貝進去運行看看吧。（可以對比 levmar-2.6 裏面的 expfit.c ）
// testlevmar.cpp : Defines the entry pointfor the console application.
//
////////////////////////////////////////////////////////////////////////////////////
// Example program that shows how to use levmar in order to fit the three-
// parameter exponential model x_i = p[0]*exp(-p[1]*i) + p[2] to a set of
// data measurements; example is based on a similar one from GSL.
//
// Copyright (C) 2008-11 ManolisLourakis (lourakis at ics forth gr)
// Institute of Computer Science, Foundation for Research & Technology- Hellas
// Heraklion, Crete, Greece.
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
////////////////////////////////////////////////////////////////////////////////////
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "levmar.h"
#ifdef _cplusplus
extern "C"{
#include "f2c.h"
#include "clapack.h"
}
#endif
#ifndef LM_DBL_PREC
#error Example program assumes that levmarhas been compiled with double precision, see LM_DBL_PREC!
#endif
#define M_PI 3.14
#undef REPEATABLE_RANDOM
#define DBL_RAND_MAX (double)(RAND_MAX)
#ifdef _MSC_VER // MSVC
#include <process.h>
#define GETPID _getpid
#elif defined(__GNUC__) // GCC
#include <sys/types.h>
#include <unistd.h>
#define GETPID getpid
#else
#warning Do not know the name of thefunction returning the process id for your OS / compiler combination
#define GETPID 0
#endif
#ifdef REPEATABLE_RANDOM
#define INIT_RANDOM(seed) srandom(seed)
#else
#define INIT_RANDOM(seed)srandom((int)GETPID()) // seed unused
#endif
double gNoise(double m, double s)
{
doubler1, r2, val;
r1= ((double)rand()) / DBL_RAND_MAX;
r2= ((double)rand()) / DBL_RAND_MAX;
val= sqrt(-2.0*log(r1))*cos(2.0*M_PI*r2);
val= s*val + m;
returnval;
}
struct xtradata{
charmsg[128];
};
void expfunc(double *p, double *x, int m,int n, void *data)
{
registerint i;
structxtradata *dat;
dat= (struct xtradata *)data;
for(i = 0; i<n; ++i)
{
x[i]= p[0] * exp(-p[1] * i) + p[2];
}
}
/* Jacobian of expfunc() */
void jacexpfunc(double *p, double *jac, intm, int n, void *data)
{
registerint i, j;
structxtradata *dat;
dat= (struct xtradata *)data;
/*fill Jacobian row by row */
for(i = j = 0; i<n; ++i)
{
jac[j++]= exp(-p[1] * i);
jac[j++]= -p[0] * i*exp(-p[1] * i);
jac[j++] = 1.0;
}
}
int main()
{
constint n = 40, m = 3; // 40 measurements, 3 parameters
doublep[m], x[n], opts[LM_OPTS_SZ], info[LM_INFO_SZ];
registerint i;
intret;
structxtradata data;
/*generate some measurement using the exponential model with
*parameters (5.0, 0.1, 1.0), corrupted with zero-mean
*Gaussian noise of s=0.1
*/
//INIT_RANDOM(0);//這個地方源程序是沒註釋掉的
for(i = 0; i<n; ++i)
x[i]= (5.0*exp(-0.1*i) + 1.0) + gNoise(0.0, 0.1);
/*initial parameters estimate: (1.0, 0.0, 0.0) */
p[0]= 1.0; p[1] = 0.0; p[2] = 0.0;
/*optimization control parameters; passing to levmar NULL instead of opts revertsto defaults */
opts[0]= LM_INIT_MU; opts[1] = 1E-15; opts[2] = 1E-15; opts[3] = 1E-20;
opts[4]= LM_DIFF_DELTA; // relevant only if the finite difference Jacobian version isused
strcpy_s(data.msg,"Hello there!");
/*invoke the optimization function */
ret= dlevmar_der(expfunc, jacexpfunc, p, x, m, n, 1000, opts, info, NULL, NULL,(void *)&data); // with analytic Jacobian
//ret=dlevmar_dif(expfunc,p, x, m, n, 1000, opts, info, NULL, NULL, (void *)&data); // withoutJacobian
printf("Levenberg-Marquardtreturned in %g iter, reason %g, sumsq %g [%g]\n", info[5], info[6],info[1], info[0]);
printf("Bestfit parameters: %.7g %.7g %.7g\n", p[0], p[1], p[2]);
system("pause");
exit(0);
}